﻿#include "mainwindow.h"
#include "ui_mainwindow.h"

#pragma execution_character_set("utf-8")

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    //机械臂模块初始化
    robot_monitor_thread_ = new QThread();
    robot_arm_monitor_ = new RobotArmMonitor();
    robot_arm_monitor_->moveToThread(robot_monitor_thread_);

    //设置机械臂监控器

    mainthread_robot_arm_ = new RobotArm();
    QString ip = ui->line_edit_ads_robotarm->text();
    mainthread_robot_arm_->set_robot_monitor(robot_arm_monitor_,ip);

    connect(robot_monitor_thread_,&QThread::finished, robot_arm_monitor_,&QObject::deleteLater);
    bool state = connect(this,SIGNAL(sig_start_monitor()),robot_arm_monitor_,SLOT(run_monitor()));
    connect(robot_arm_monitor_,SIGNAL(sig_finish()),this,SLOT(slot_finish()));

    robot_monitor_thread_->start();

    //timer
    timer_get_robot_pose_ = new QTimer(this);
    connect(timer_get_robot_pose_,&QTimer::timeout,this,&MainWindow::timer_getRobotPose);
    timer_get_robot_pose_->start(500);

    //timer

    timer_get_agv_state_ = new QTimer(this);
    connect(timer_get_agv_state_,&QTimer::timeout,this,&MainWindow::timer_getAGVState);
    //timer_get_agv_state_->start(500);


    //光源串口初始化
    SerialPortInit();

    //初始化DataMatrix模块
    Initial_encoding_format();

    //相机模块初始化
    camera_ = new Camera();

    timer_get_camera_image_ = new QTimer(this);
    connect(timer_get_camera_image_,&QTimer::timeout,this,&MainWindow::timer_getCameraImage);
    //timer_get_camera_image_->start(500);

    //modbus模块初始化

    //流程类初始化
    /*
    work_flow_ = new WorkFlow(Q_NULLPTR,
                              camera_,
                              mainthread_robot_arm_,
                              &image_processor_,
                              &NDC_Client_);

    work_flow_pthread_ = new QThread();
    connect(work_flow_pthread_,&QThread::finished, work_flow_,&QObject::deleteLater);
    connect(this,SIGNAL(sig_dowork()),work_flow_,SLOT(run_work_flow()));
    connect(work_flow_,SIGNAL(sig_finish()),this,SLOT(slot_finish()));

    work_flow_->moveToThread(work_flow_pthread_);
    work_flow_->set_main_thread(this);

    work_flow_pthread_->start();
    */


    //设置测试状态
    set_test_qlabel();

    //设置事件过滤器
    ui->label_show->installEventFilter(this);

    //
    timer_work_flow_ = new QTimer(this);
    connect(timer_work_flow_,&QTimer::timeout,this,&MainWindow::timer_work_flow_loop);

    ui->tableWidget_log->setColumnWidth(1,700);
}

MainWindow::~MainWindow()
{
    delete ui;

    //delete work_flow_;
    //delete pthread_;
    camera_->turnOffCamera(CameraName::RobotArm);
    camera_->turnOffCamera(CameraName::DataMatrix);
    delete camera_;

    //光源模块关闭处理
    //qDebug() << "closeLight1";
    light_->closeLight1();
    //qDebug() << "closeLight2";
    light_->closeLight2();
    delete light_;

    //qDebug() << "disconnect robotarm";
    mainthread_robot_arm_->disconnectToHost();

    sleep_ms(1000);

}

void MainWindow::slot_finish(){

}


void MainWindow::on_OpenMonitor_clicked()
{
   return;
}

void MainWindow::on_closeMonitor_clicked()
{
   return;
}

void MainWindow::on_turnOnCamera_clicked()
{
   //camera_.turnOnCamera();
}

void MainWindow::on_turnOffCamera_clicked()
{
   //camera_.turnOffCamera();
}


void MainWindow::timer_getRobotPose()
{
   //robot_arm_.getRobotBaseFlangePose();
   // show ui;

    RobotPoseJoint pose_joint = mainthread_robot_arm_->getRobotPoseJoint();

    RobotPoseXYZ pose_xyz = mainthread_robot_arm_->getRobotPoseXYZ();

    ui->label_position_x->setText(QString::number(pose_xyz.x , 'f', 6 ));
    ui->label_position_y->setText(QString::number(pose_xyz.y , 'f', 6 ));
    ui->label_position_z->setText(QString::number(pose_xyz.z , 'f', 6 ));
    ui->label_position_rx->setText(QString::number(pose_xyz.rx , 'f', 6 ));
    ui->label_position_ry->setText(QString::number(pose_xyz.ry , 'f', 6 ));
    ui->label_position_rz->setText(QString::number(pose_xyz.rz , 'f', 6 ));

    ui->label_joint_1->setText(QString::number(pose_joint.joint1 , 'f', 6 ));
    ui->label_joint_2->setText(QString::number(pose_joint.joint2 , 'f', 6 ));
    ui->label_joint_3->setText(QString::number(pose_joint.joint3 , 'f', 6 ));
    ui->label_joint_4->setText(QString::number(pose_joint.joint4 , 'f', 6 ));
    ui->label_joint_5->setText(QString::number(pose_joint.joint5 , 'f', 6 ));
    ui->label_joint_6->setText(QString::number(pose_joint.joint6 , 'f', 6 ));

}


void MainWindow::timer_getAGVState(){

    //qDebug() << "timer_getAGVState";

    if(work_flag == false){
        //没有启动流程，直接返回
        return;
    }

    //监测agv避障信号
     int having_obstacle = 0;
     bool receive = mainthread_robot_arm_->get_DI(13,having_obstacle);


     if(having_obstacle == 1){

         //如果已经检测到障碍物
         if(obstacle_state == true){
             //更新机器人可以恢复运动的时间
             obstacle_outTime = QTime::currentTime().addMSecs(5000);
             return;
         }

         if(mainthread_robot_arm_->pause_movement()){
             obstacle_state = true;
             //设置下次机器人可以恢复运动的时间
             obstacle_outTime = QTime::currentTime().addMSecs(5000);
         }
     }
     else{

         //当前处于暂停状态
         if(obstacle_state == true){

             //同时满足机器人可以恢复运动的时间
             if(QTime::currentTime() > obstacle_outTime ){
                 QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

                 if(mainthread_robot_arm_->run_movement()){
                     obstacle_state = false;
                 }

             }
         }

     }

}


void MainWindow::timer_getCameraImage(){

    if(!camera_->isOpen(CameraName::RobotArm)){
        //QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::RobotArm, image);
    QR_image = image.clone();

    cv::Mat QTmat = image.clone();
    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

    QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                           QTmat.cols, QTmat.rows, QTmat.step,
                           QImage::Format_RGB888);

    int width = ui->label_show->width();
    int height = ui->label_show->height();

    qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                     Qt::IgnoreAspectRatio,
                                                                     Qt::SmoothTransformation);
    this->update();
}


void MainWindow::getCameraImage()
{
    //cv::Mat image;
    //camera_.GetImage(image);

    //QTime time;

    //time.start();

}


void MainWindow::on_connectButton_clicked()
{

        const QUrl url = QUrl::fromUserInput(ui->portEdit->text());

        std::string host = url.host().toStdString();
        int port = url.port();

        bool state = NDC_Client_.connectToHost(host, port);

    if(state == true){
        QMessageBox::information(this,"提示\t","成功连接到Modbus\t");
    }
    else{
        QMessageBox::information(this,"提示\t","连接到Modbus失败\t");
    }

}

void MainWindow::on_readButton_clicked()
{

    if(!NDC_Client_.check_Modbus_Available())
    {
        QMessageBox::information(this,"提示\t","未连接到Modbus\t");
        return;
    }


    bool state = NDC_Client_.read_AGV_State();

    if(state == false){

        QMessageBox::information(this,"提示\t","读取数据失败\t");
        return;
    }else{

        //sleep_ms(1000);
        int value;
        if(NDC_Client_.Func_AGV_arrive_station_number(value)){
                ui->label_modbus_address_52->setNum(value);
        }

        //sleep_ms(1000);
        if(NDC_Client_.Func_allow_put_raw_workpiece_to_CNC(value)){
                ui->label_modbus_address_53->setNum(value);
        }

        //sleep_ms(1000);
        if(NDC_Client_.Func_allow_take_workpiece_from_CNC(value)){
                ui->label_modbus_address_54->setNum(value);
        }

        //sleep_ms(1000);
        if(NDC_Client_.Func_allow_take_raw_workpiece_from_lifter(value)){
                ui->label_modbus_address_55->setNum(value);
        }

        //sleep_ms(1000);
        if(NDC_Client_.Func_allow_put_workpiece_to_lifter(value)){
                ui->label_modbus_address_56->setNum(value);
        }

        if(NDC_Client_.Func_get_AGV_state(value)){
                ui->label_modbus_address_57->setNum(value);
        }

        //ui->label_modbus_address_57->setNum(0);
    }


}

void MainWindow::on_writeButton_clicked()
{
    //int type_index = ui->writeTable->currentData().toInt();

    //modbus_tcp_->modbus_write_coil(0,true);
    //modbus_tcp_->modbus_write_coils();
    //modbus_tcp_->modbus_write_register();
    //modbus_tcp_->modbus_write_registers();
}

void MainWindow::on_startFlowButton_clicked()
{
    //起始点检测
    bool is_start = mainthread_robot_arm_->is_current_pose("初始化待机点",0.05);

    if(is_start == false){
         stop_work_flow();
         qDebug() << "请移动到初始化待机点";
         QMessageBox::information(this, "提示\t", "请移动到初始化待机点\t");
        return;
    }

    //机械臂模式检测
    bool remotemode = mainthread_robot_arm_->is_remote_mode();

    if(remotemode == false){
         stop_work_flow();
         qDebug() << "请切换机械臂到远程模式";
         QMessageBox::information(this, "提示\t", "请切换机械臂到远程模式\t");
        return;
    }

        //清除所有路点
        qDebug() << "清除所有路点";
        mainthread_robot_arm_->clear_waypoints();

        //故障红灯显示清除
        qDebug() << "故障红灯显示清除";
        NDC_Client_.write_AGV_data_69(0);

        //二次定位平台物料清除
        qDebug() << "二次定位平台物料清除";
        has_workpeice_in_localization = false;

        ui->startFlowButton->setDisabled(true);
        ui->groupBox_Test1->setDisabled(true);
        ui->groupBox_Test2->setDisabled(true);
        ui->groupBox_Test3->setDisabled(true);

        ui->tab->setDisabled(true);
        ui->tab_1->setDisabled(true);
        ui->tab_2->setDisabled(true);
        ui->tab_3->setDisabled(true);
        ui->tab_4->setDisabled(true);
        ui->tab_5->setDisabled(true);
        ui->tab_6->setDisabled(true);
        ui->tab_7->setDisabled(true);
        //ui->tab_8->setDisabled(true);
        ui->tab_9->setDisabled(true);
        ui->tab_10->setDisabled(true);

        ui->button_load_all_station_data->setDisabled(true);

        qDebug() << "开启流程";
        work_flag = true;
        timer_work_flow_->start(500);


}

void MainWindow::on_button_add_common_pos_clicked()
{
    RobotPoseJoint data = mainthread_robot_arm_->getRobotPoseJoint();

    int countIndex = ui->tableWidget_robotarm_common_pos->rowCount();

    ui->tableWidget_robotarm_common_pos->setRowCount(countIndex + 1);
    ui->tableWidget_robotarm_common_pos->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

    ui->tableWidget_robotarm_common_pos->setItem(countIndex,1,new QTableWidgetItem(QString::number(data.joint1 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(countIndex,2,new QTableWidgetItem(QString::number(data.joint2 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(countIndex,3,new QTableWidgetItem(QString::number(data.joint3 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(countIndex,4,new QTableWidgetItem(QString::number(data.joint4 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(countIndex,5,new QTableWidgetItem(QString::number(data.joint5 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(countIndex,6,new QTableWidgetItem(QString::number(data.joint6 , 'f', 6 )));

    QMessageBox::information(this, "提示\t", "命名点位，最后选择保存\t");

}

void MainWindow::on_button_save_common_pos_clicked()
{
    //QMessageBox::information(this, "提示\t", "开始保存数据\t");

    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "RobotArmPose\\Common";

    QDir dir(DirPath+DirName);

    if (!dir.exists())
    {
        dir.mkdir(DirPath+DirName);
    }

    QFile RobotPoseData(DirPath + DirName +"\\robot_common_pose.json");

    //| QIODevice::Append
    if(!RobotPoseData.open(QIODevice::WriteOnly | QIODevice::Text ))
    {

        return;
    }

    QJsonDocument jsonDoc;
    int rows = ui->tableWidget_robotarm_common_pos->rowCount();

    QJsonObject jsonObj;
    for(int i=0; i<rows; i++){

        //record data
        bool data_available = true;
        QJsonArray array;
        for(int j=0; j<6; j++){

           double value = 0.0;
           if(ui->tableWidget_robotarm_common_pos->item(i,j+1) == NULL){
               data_available = false;
               break;
           }
           value = ui->tableWidget_robotarm_common_pos->item(i,j+1)->text().toDouble();
           array.append(value);
        }

        //record name
        bool name_available = true;
        QString name;
        if(ui->tableWidget_robotarm_common_pos->item(i,0) == NULL){
            name_available = false;
        }else{
            name = ui->tableWidget_robotarm_common_pos->item(i,0)->text();
        }

        if(!name.isEmpty()){
            jsonObj.insert(name,array);

            RobotPoseJoint joint_pose = { array.at(0).toDouble(),array.at(1).toDouble(),
                                          array.at(2).toDouble(),array.at(3).toDouble(),
                                          array.at(4).toDouble(),array.at(5).toDouble()};

            mainthread_robot_arm_->add_common_pose(name, joint_pose);
        }
    }

    jsonDoc.setObject(jsonObj);
    RobotPoseData.write(jsonDoc.toJson());
    RobotPoseData.close();
    QMessageBox::information(this, "提示\t", "数据保存完成\t"); 

}

void MainWindow::on_button_moveto_pos_pressed()
{
    int row = ui->tableWidget_robotarm_common_pos->currentRow();
    QString pose_name =  ui->tableWidget_robotarm_common_pos->item(row,0)->text();

    mainthread_robot_arm_->move_to_pose(MoveType::JOINT,pose_name);
}

void MainWindow::on_button_moveto_pos_released()
{
   mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_button_connect_to_robotarm_clicked()
{
    QString host = ui->line_edit_ads_robotarm->text();
    QString port = ui->line_edit_port_robotarm->text();
    bool state = mainthread_robot_arm_->connectToHost(host.toStdString(),port.toInt());

    if(state == true){
        QMessageBox::information(this,"Conection","Connect Success!");
        //ui->button_connect_to_robotarm->setChecked(false);
        emit sig_start_monitor();
    }
    else{
        QMessageBox::warning(this,"Conection","Connect Fail!");
    }

}

void MainWindow::SerialPortInit(){

    // 获取计算机中有效的端口号，然后将端口号的名称给端口选择控件
    QSerialPort serial;
    foreach(const QSerialPortInfo &info,QSerialPortInfo::availablePorts())
    {
        serial.setPort(info);                      // 在对象中设置串口
        if(serial.open(QIODevice::ReadWrite))      // 以读写方式打开串口
        {
            ui->comboBox_Port->addItem(info.portName());  // 添加计算机中的端口
            serial.close();                        // 关闭
        } else
        {
            qDebug() << "串口打开失败，请重试";
        }
    }

    // 参数配置
    // 波特率，波特率默认选择57600 ，禁止用户点击
    ui->comboBox_Baud->addItem("9600");
    //serial->setBaudRate(QSerialPort::Baud57600);
    //ui->BaudBox->setDisabled(true);

    // 校验，校验默认选择无
    ui->comboBox_Parity->addItem("None");
    //serial->setParity(QSerialPort::NoParity);

    // 数据位，数据位默认选择8位
    ui->comboBox_Bit->addItem("8");
    //serial->setDataBits(QSerialPort::Data8);

    // 停止位，停止位默认选择1位
    ui->comboBox_Stop->addItem("1");
    //serial->setStopBits(QSerialPort::OneStop);

    // 控制流，默认选择无
    ui->comboBox_Control->addItem("None");
    //serial->setFlowControl(QSerialPort::NoFlowControl);

    //std::string port;  //端口号
    //std::string baud;  //波特率
    //std::string dataBit; //数字位
    //std::string stopBits; //停止位
    //std::string parity; //校验位

    //SerialParam param = {"COM8", "9600", "8", "1", "None"};

    //light_->setSerialParam(param);

    light_ = new Light();

    // 刷新串口
    //RefreshSerialPort(0);

    // 信号
//connect(serial,&QSerialPort::readyRead,this,&MainWindow::DataReceived);      // 接收数据
//connect(ui->SendWordOrder,&QPushButton::clicked,this,&MainWindow::DataSend); // 发送数据
//connect(ui->SendButton,&QPushButton::clicked,this,&MainWindow::DataSend);    // 发送数据
//connect(ui->SendEditBtn1,&QPushButton::clicked,this,&MainWindow::DataSend);  // 发送数据
//connect(ui->SendEditBtn2,&QPushButton::clicked,this,&MainWindow::DataSend);  // 发送数据
//connect(ui->SendEditBtn3,&QPushButton::clicked,this,&MainWindow::DataSend);  // 发送数据

}

void MainWindow::on_pushButton_openSerial_clicked()
{
    SerialParam param;
    param.port = ui->comboBox_Port->currentText();
    param.baud = ui->comboBox_Baud->currentText();
    param.dataBit = ui->comboBox_Bit->currentText();
    param.stopBits = ui->comboBox_Stop->currentText();
    param.parity = ui->comboBox_Parity->currentText();

    if(light_->setSerialParam(param)){
        QMessageBox::information(this,"Light","Open Light Success!");
    }
    else{
        QMessageBox::information(this,"Light","Open Light Fail!");
    }
}

void MainWindow::on_pushButton_send_clicked()
{
    QString data = ui->textEdit_dataSend->toPlainText();

    //light_.openLight(data);
}


void MainWindow::on_pushButton_openLight_clicked()
{
    
}

void MainWindow::on_pushButton_closeLight_clicked()
{
    
}

void MainWindow::on_pushButton_collision_recover_clicked()
{
     bool state = mainthread_robot_arm_->syncMotorStatus();

     if(state == false){
         QMessageBox::information(this,"RecoverServo","Recover Servo Fail!");
         return;
     }

     state = mainthread_robot_arm_->set_servo_status(true);

     if(state == false){
         QMessageBox::information(this,"RecoverServo","Recover Servo Fail!");
         return;
     }else{
         QMessageBox::information(this,"RecoverServo","Recover Servo Success!");
         return;
     }

}

void MainWindow::on_pushButton_control_log_in_clicked()
{
    bool state = mainthread_robot_arm_->robotArmInit();

    if(state == true){
        QMessageBox::information(this,"Robot Arm Init","Robot Arm Inilization Success!");
    }
    else{
        QMessageBox::warning(this,"Robot Arm Init","Robot Arm Inilization Fail!");
    }
}

void MainWindow::on_pushButton_control_power_on_clicked()
{

}

void MainWindow::on_button_load_common_pos_clicked()
{
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "RobotArmPose\\Common";

    QString RobotPoseData_Dir = DirPath + DirName +"\\robot_common_pose.json";
    //打开文件
    QFile file(RobotPoseData_Dir);
    if(!file.exists())
    {
        QMessageBox::critical(this, "错误\t", "找不到该文件!\t");
        return;
    }
    file.open(QIODevice::ReadOnly);
    QByteArray data=file.readAll();
    //file.close();
    //使用json文件对象加载字符串
    QJsonDocument doc=QJsonDocument::fromJson(data);
    //判断是否对象
    if(doc.isObject())
    {
        //把json文档转换为json对象
        QJsonObject obj=doc.object();

        for(const QString& key : obj.keys()){

            QJsonValue jvalue = obj[key];
            double pose[6];
            if(jvalue.isArray()){
                QJsonArray array = jvalue.toArray();
                int nSize = array.size();

                for(int i=0; i < nSize; ++i){
                    QJsonValue value = array.at(i);
                    if(value.isDouble()){
                        pose[i] = value.toDouble();
                    }
                }
            }

            RobotPoseJoint joint_pose ={
                pose[0],
                pose[1],
                pose[2],
                pose[3],
                pose[4],
                pose[5]
            };

            mainthread_robot_arm_->add_common_pose(key,joint_pose);

            //write to table
            int countIndex = ui->tableWidget_robotarm_common_pos->rowCount();

            ui->tableWidget_robotarm_common_pos->setRowCount(countIndex + 1);
            ui->tableWidget_robotarm_common_pos->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

            ui->tableWidget_robotarm_common_pos->setItem(countIndex,0,new
                                                  QTableWidgetItem(key));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,1,
                                                  new QTableWidgetItem(QString::number(pose[0],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,2,
                                                  new QTableWidgetItem(QString::number(pose[1],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,3,
                                                  new QTableWidgetItem(QString::number(pose[2],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,4,
                                                  new QTableWidgetItem(QString::number(pose[3],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,5,
                                                  new QTableWidgetItem(QString::number(pose[4],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,6,
                                                  new QTableWidgetItem(QString::number(pose[5],'f',6)));
        }

    }
}

void MainWindow::on_pushButton_location_z_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToZMinus();
}

void MainWindow::on_pushButton_location_z_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToZPlus();
}

void MainWindow::on_pushButton_location_x_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToXMinus();
}

void MainWindow::on_pushButton_location_x_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToXPlus();
}

void MainWindow::on_pushButton_location_y_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToYMinus();
}

void MainWindow::on_pushButton_location_y_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToYPlus();
}

void MainWindow::on_pushButton_location_z_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_location_z_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_location_x_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_location_x_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_location_y_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_location_y_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_1_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint1Plus();
}

void MainWindow::on_pushButton_joint_1_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint1Minus();
}

void MainWindow::on_pushButton_joint_2_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint2Plus();
}

void MainWindow::on_pushButton_joint_2_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint2Minus();
}

void MainWindow::on_pushButton_joint_3_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint3Plus();
}

void MainWindow::on_pushButton_joint_3_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint3Minus();
}

void MainWindow::on_pushButton_joint_4_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint4Plus();
}

void MainWindow::on_pushButton_joint_4_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint4Minus();
}

void MainWindow::on_pushButton_joint_5_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint5Plus();
}

void MainWindow::on_pushButton_joint_5_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint5Minus();
}

void MainWindow::on_pushButton_joint_6_up_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint6Plus();
}

void MainWindow::on_pushButton_joint_6_down_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->moveToJoint6Minus();
}

void MainWindow::on_pushButton_joint_1_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_1_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_2_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_2_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_3_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_3_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_4_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_4_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_5_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_5_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_6_up_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_joint_6_down_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

bool MainWindow::calculateCenter()
{
    /*
    //图像更新
    cv::Mat image;

    //旋转间隔
    //int rotate_interval = ui->spinBox_rotateInterval->value();
    //旋转次数
    //int rotate_num; //= ui->spinBox_rotateTime->value();

    //获得当前机械臂坐标
    RobotPoseJoint robot_pose = mainthread_robot_arm_->getRobotPoseJoint();

    //检测机械臂坐标获取是否准确
    bool isGet = true;
    if( robot_pose.joint1 == 0
        && robot_pose.joint2 == 0
        && robot_pose.joint3 == 0){
        isGet = false;
    }

    //拷贝一份示教原点的数据
    float position_origin[6] = { robot_pose.joint1, robot_pose.joint2, robot_pose.joint3,
                                 robot_pose.joint4, robot_pose.joint5, robot_pose.joint6 };

    float position_temp[6] = { robot_pose.joint1, robot_pose.joint2, robot_pose.joint3,
                               robot_pose.joint4, robot_pose.joint5, robot_pose.joint6 };

    //如果获得当前点位
    RobotPoseJoint pose_tomove;
    if (true == isGet)
    {
        //旋转最后一个轴
        position_temp[5] = position_temp[5] - rotate_interval*rotate_num*3.141592/360;
        //移动到计算旋转中心的起点
        pose_tomove = {position_temp[0], position_temp[1], position_temp[2],
                       position_temp[3], position_temp[4], position_temp[5]};
        mainthread_robot_arm_->move_to_pose(pose_tomove,
                                           MoveType::JOINT,
                                           SPEED_5,
                                           robot::ACC_5,
                                           robot::DEC_5,
                                           BlockMotion::LOCKED);
        //等待2秒使机械手稳定
        mainthread_robot_arm_->robot_sleep(2000);

        //collect the data into points;
        std::vector<cv::Point2d> points;

        for (int i = 0; i < rotate_num; i++)
        {
            //旋转最后一个轴
            position_temp[5] = position_temp[5] + rotate_interval*3.141592/180;
            //移动点位
            pose_tomove = {position_temp[0], position_temp[1], position_temp[2],
                           position_temp[3], position_temp[4], position_temp[5]};
            mainthread_robot_arm_->move_to_pose(pose_tomove,
                                               MoveType::JOINT,
                                               SPEED_5,
                                               robot::ACC_5,
                                               robot::DEC_5,
                                               BlockMotion::LOCKED);

            //等待2秒使机械手稳定
            mainthread_robot_arm_->robot_sleep(2000);

            //获取图像
            camera_->GetImage(CameraName::RobotArm, image);

            //二维码检测
            cv::Point2d pixel;
            std::vector<cv::Point2d> corners;
            float angle;
            bool isFind = image_processor_.detectQRLandmark(image,
                                                            corners,
                                                            pixel,
                                                            angle);

            if (true == isFind)
            {
                points.push_back(pixel);

                //绘画并显示
                camera_->GetImage(CameraName::RobotArm, image);
                if(!image.empty())
                {
                    cv::Mat QTmat = image.clone();
                    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB); //CV_GRAY2RGB

                    for(int j=0; j< points.size(); j++)
                    {
                        cv::Point p2;
                        p2.x = points[j].x;
                        p2.y = points[j].y;
                        int size = image.size().height * 0.05;

                        //cv::circle(Image, p2, 3, cv::Scalar(0,0,255),5,-1); //第五个参数我设为-1，表明这是个实点。
                        cv::line(QTmat,cv::Point(p2.x-size/2,p2.y),cv::Point(p2.x+size/2,p2.y),cv::Scalar(0,255,0),2,8,0);
                        cv::line(QTmat,cv::Point(p2.x,p2.y-size/2),cv::Point(p2.x,p2.y+size/2),cv::Scalar(0,255,0),2,8,0);

                    }

                    QImage qcolor = QImage((const unsigned char*)(QTmat.data), QTmat.cols, QTmat.rows, QTmat.step, QImage::Format_RGB888);

                    int width = ui->label_show->width();
                    int height = ui->label_show->height();

                    QImage qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
                    ui->label_show->setPixmap(QPixmap::fromImage(qcolorshow));
                    ui->label_show->resize(qcolorshow.size());

                }
            }
        }

        //移动到示教时的原点
        pose_tomove = {position_origin[0], position_origin[1], position_origin[2],
                       position_origin[3], position_origin[4], position_origin[5]};
        mainthread_robot_arm_->move_to_pose(pose_tomove,
                                           MoveType::JOINT,
                                           SPEED_5,
                                           robot::ACC_5,
                                           robot::DEC_5,
                                           BlockMotion::LOCKED);
        //等待2秒使机械手稳定
        mainthread_robot_arm_->robot_sleep(3000);

        //计算旋转中心
        if (points.size() > 4)
        {

            double radius;
            cv::Point2d mark_center;
            calibration_.circleLeastFit(points,mark_center,radius);

            //
            cv::Point p2;
            p2.x = mark_center.x;
            p2.y = mark_center.y;

            cv::Mat QTmat = image.clone();
            cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

            cv::circle(QTmat, p2, 3, cv::Scalar(0,0,255),10,-1); //第五个参数我设为-1，表明这是个实点。
            cv::circle(QTmat, p2, radius, cv::Scalar(0,0,255),3,cv::LINE_8); //第五个参数我设为-1，表明这是个实点。

            QImage qcolor = QImage((const unsigned char*)(QTmat.data), QTmat.cols, QTmat.rows, QTmat.step, QImage::Format_RGB888);

            int width = ui->label_show->width();
            int height = ui->label_show->height();

            QImage qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
            ui->label_show->setPixmap(QPixmap::fromImage(qcolorshow));
            ui->label_show->resize(qcolorshow.size());

            //robot_arm_.robot_sleep(3000);
            //QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("圆拟合完成"));
            QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("旋转中心计算完成"));
            return true;
        }
        else
        {
            QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("少于4个点，请重新示教模板!!!"));
            return false;
        }
    }
    else
    {
        QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("机械臂移动异常，请检查!!!"));
        return false;
    }
    */
    return true;
}


bool MainWindow::calculateTransformMatrix(){

    //clear old data
    center_nine_points.clear();

    //input
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();
    QString readDir = DirPath + DirName + station_id;

    //清除表中数据
    ui->tableWidget_autoCalibration->clearContents();
    //X和Y方向的偏移值
    double x_shift_interval = ui->spinBox_x_interval->value();///1000.0;
    int x_shift_number = 3;
    double y_shift_interval = ui->spinBox_x_interval->value();///1000.0;
    int y_shift_number = 3;

    //设置曝光时间
    //m_visionPro->setExposuretime(ExposureStation, "Mark", 1);


    //检测机械臂坐标获取是否正确
    bool isGet = false;
    RobotPoseXYZ current_pose = mainthread_robot_arm_->getRobotPoseXYZ();
    if(current_pose.x != 0 && current_pose.y != 0)
    {
       isGet = true;
    }

    //获得当前机械手坐
    float position_temp[6] = {current_pose.x, current_pose.y, current_pose.z,
                              current_pose.rx, current_pose.ry, current_pose.rz};

    //运动九点坐
    float points_position[9][6];
    //点位的运动轨迹
    float points_derection[9][2] = {{0.0},     {1.0},     {1,-1},
                                    {0,-1},    {-1,-1},    {-1,0},
                                    {-1,1},     {0,1},     {1,1}};
    //为九点赋值
    for (int i = 0; i < 9; i++)
    {
        points_position[i][0] = points_derection[i][0] * x_shift_interval + position_temp[0];
        points_position[i][1] = points_derection[i][1] * y_shift_interval + position_temp[1];
        points_position[i][2] = position_temp[2];
        points_position[i][3] = position_temp[3];
        points_position[i][4] = position_temp[4];
        points_position[i][5] = position_temp[5];
    }

    RobotPoseXYZ pose_tomove;

    if (true == isGet)
    {
        int row_index, fail_count;
        fail_count = 0;

        std::vector<cv::Point2d> points;

        //获取图像
        cv::Mat image;
        camera_->GetImage(CameraName::RobotArm, image);

        //检测二维码
        std::vector<cv::Point2d> base_corners;
        cv::Point2d base_center;
        float base_angle;

        bool isFind = image_processor_.match_image_model(readDir , station_id,
                                           image,
                                           base_corners,
                                           base_center,
                                           base_angle);

        if(base_center.x == 0 && base_center.y == 0){
             QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("base_center Fail"));
             return false;
        }

        //获取旋转中心点
        cv::Point2d mark_center = calibration_.getMarkCenter();

        double offset_center_x = mark_center.x - base_center.x;
        double offset_center_y = mark_center.y - base_center.y;

        for (int i = 0; i < 9; i++)
        {
            //移动到目标点
            pose_tomove = {points_position[i][0], points_position[i][1], points_position[i][2],
                                       points_position[i][3], points_position[i][4],points_position[i][5]};
            mainthread_robot_arm_->move_to_pose(pose_tomove,
                                               MoveType::LINE,
                                               LineSpeed,LineACC,LineDEC,
                                               BlockMotion::LOCKED);

            //waitfor(3000);
            mainthread_robot_arm_->robot_sleep(500);

            //geometry_msgs::PointStamped pixel;
            camera_->GetImage(CameraName::RobotArm, image);

            std::vector<cv::Point2d> corners;
            cv::Point2d center;
            float angle;

            bool isFind = image_processor_.match_image_model(readDir , station_id,
                                               image,
                                               corners,
                                               center,
                                               angle);

            center_nine_points.push_back(center);

            if (true == isFind)
            {
                int row_index = i;
                ui->tableWidget_autoCalibration->setItem(row_index, 0, new QTableWidgetItem(QString::number(center.x + offset_center_x)));
                ui->tableWidget_autoCalibration->setItem(row_index, 1, new QTableWidgetItem(QString::number(center.y + offset_center_y)));
                ui->tableWidget_autoCalibration->setItem(row_index, 2, new QTableWidgetItem(QString::number(2 * position_temp[0] - points_position[i][0])));
                ui->tableWidget_autoCalibration->setItem(row_index, 3, new QTableWidgetItem(QString::number(2 * position_temp[1] - points_position[i][1])));

                //refresh points
                camera_->GetImage(CameraName::RobotArm, image);
                if(!image.empty())
                {
                    cv::Mat QTmat = image.clone();
                    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

                    for(int j=0; j< center_nine_points.size(); j++)
                    {
                        cv::Point p2;
                        p2.x = center_nine_points[j].x;
                        p2.y = center_nine_points[j].y;
                        int size = image.size().height * 0.05;

                        //cv::circle(Image, p2, 3, cv::Scalar(0,0,255),5,-1); //第五个参数我设为-1，表明这是个实点。
                        cv::line(QTmat,cv::Point(p2.x-size/2,p2.y),cv::Point(p2.x+size/2,p2.y),cv::Scalar(0,255,0),2,8,0);
                        cv::line(QTmat,cv::Point(p2.x,p2.y-size/2),cv::Point(p2.x,p2.y+size/2),cv::Scalar(0,255,0),2,8,0);

                    }

                    QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                                           QTmat.cols, QTmat.rows, QTmat.step,
                                           QImage::Format_RGB888);

                    int width = ui->label_show->width();
                    int height = ui->label_show->height();

                    qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                                     Qt::IgnoreAspectRatio,
                                                                                     Qt::SmoothTransformation);
                    this->update();

                }
            }
            else
            {
                fail_count++;
            }
            //更新新的位置
        }

        //计算九点
        bool isDone = autoCalibrateImageBoard();
        //默认自动保存九点标定数据
        if (isDone)
        {
            //回到原示教点位
            RobotPoseXYZ pose = {position_temp[0], position_temp[1], position_temp[2],
                                   position_temp[3], position_temp[4],position_temp[5]};
            mainthread_robot_arm_->move_to_pose(pose,
                                               MoveType::LINE,
                                               LineSpeed,LineACC,LineDEC,
                                               BlockMotion::LOCKED);
            //等待3秒
            mainthread_robot_arm_->robot_sleep(3000);

        }

        //恢复butoon
        ui->pushButton_auto_calibration->setEnabled(true);

        QMessageBox::information(this, QStringLiteral("mention"), QStringLiteral("nine points calibration done"));
        return true;
    }
    else
    {
        QMessageBox::information(this, QStringLiteral("mention"), QStringLiteral("robot movement error!!!"));
        return false;
    }
}

bool MainWindow::autoCalibrateImageBoard()
{
    double value;
    int Column = 4;
    int Row = 9;
    QTableWidgetItem *item;

    std::vector<cv::Point2f> srcTri;
    std::vector<cv::Point2f> dstTri;

    for (int i = 0; i <= Row - 1; i++)
    {
        //检测表中是否有数据
        item = ui->tableWidget_autoCalibration->item(i, 0);
        if (!item)
            continue;

        double pixel_x = ui->tableWidget_autoCalibration->item(i, 0)->text().toDouble();
        double pixel_y = ui->tableWidget_autoCalibration->item(i, 1)->text().toDouble();
        double world_x = ui->tableWidget_autoCalibration->item(i, 2)->text().toDouble();
        double world_y = ui->tableWidget_autoCalibration->item(i, 3)->text().toDouble();

        srcTri.push_back(cv::Point2f(pixel_x,pixel_y));
        dstTri.push_back(cv::Point2f(world_x,world_y));

    }

    //提示是否选取足够数量点对
    if (srcTri.size() < 3)
    {
        QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("少于3组点!!!"));
        return false;
    }

    //计算仿射矩阵
    cv::Mat warp_mat(2,3,CV_32FC1);
    std::string name = ui->comboBox_station_name_matrix->currentText().toStdString();
    int index  = 0;//ui->spinBox_station_matrix->value();

    warp_mat = calibration_.getAffineTransform64f(name, index, srcTri, dstTri);

//    qDebug() << "A " << warp_mat.at<double>(0,0);
//    qDebug() << "B " << warp_mat.at<double>(0,1);
//    qDebug() << "C " << warp_mat.at<double>(0,2);
//    qDebug() << "D " << warp_mat.at<double>(1,0);
//    qDebug() << "E " << warp_mat.at<double>(1,1);
//    qDebug() << "F " << warp_mat.at<double>(1,2);

    //show data
    ui->doubleSpinBox_matrixData_A->setValue(warp_mat.at<double>(0,0));
    ui->doubleSpinBox_matrixData_B->setValue(warp_mat.at<double>(0,1));
    ui->doubleSpinBox_matrixData_C->setValue(warp_mat.at<double>(0,2));
    ui->doubleSpinBox_matrixData_D->setValue(warp_mat.at<double>(1,0));
    ui->doubleSpinBox_matrixData_E->setValue(warp_mat.at<double>(1,1));
    ui->doubleSpinBox_matrixData_F->setValue(warp_mat.at<double>(1,2));

    std::vector<cv::Point2f> outcome;
    cv::transform(srcTri,outcome,warp_mat);

    double max_error_x = 0;
    double max_error_y = 0;

    for(int i=0; i<outcome.size(); i++)
    {
        double error_x = std::abs(outcome.at(i).x - dstTri.at(i).x);
        double error_y = std::abs(outcome.at(i).y - dstTri.at(i).y);

        ui->tableWidget_autoCalibration->setItem(i, 4, new QTableWidgetItem(QString::number(error_x )));
        ui->tableWidget_autoCalibration->setItem(i, 5, new QTableWidgetItem(QString::number(error_y )));

        if(error_x > max_error_x) max_error_x = error_x;
        if(error_y > max_error_y) max_error_y = error_y;
    }

    ui->textEdit_max_x_error->setText(QString::number(max_error_x));
    ui->textEdit_max_y_error->setText(QString::number(max_error_y));

    QString station_id = ui->comboBox_station_name_matrix->currentText();
    calibration_.add_station_transform_matrix(station_id, warp_mat);

    return true;
}

void MainWindow::on_pushButton_auto_calibration_clicked()
{
    //设置button为灰
    ui->pushButton_auto_calibration->setDisabled(true);
    //默认定位工台在小车的右边

    //step1:先计算旋转中心
   //bool state = calculateCenter();

   //if(state == false) return;

    //step2:再进行计算自动nine point
    calculateTransformMatrix();

}

void MainWindow::on_pushButton_openCamera_clicked()
{

}

void MainWindow::on_pushButton_closeCamera_clicked()
{

}

void MainWindow::on_pushButton_openLight1_clicked()
{
    light_->openLight1();
}

void MainWindow::on_pushButton_closeLight1_clicked()
{
    light_->closeLight1();
}

void MainWindow::on_pushButton_openLight2_clicked()
{
    //light_->openLight2();
    light_->setLuminance2(255);
}

void MainWindow::on_pushButton_closeLight2_clicked()
{
    light_->closeLight2();
}

void MainWindow::on_pushButton_modbus_write_64_clicked()
{
    NDC_Client_.write_ADV_data_64(ui->spinBox_modbus_coil_value_64->value());

}

void MainWindow::on_pushButton_modbus_write_65_clicked()
{
    NDC_Client_.write_ADV_data_65(ui->spinBox_modbus_coil_value_65->value());
}

void MainWindow::on_pushButton_modbus_write_66_clicked()
{
    int value = ui->spinBox_modbus_coil_value_66->value();

    NDC_Client_.write_ADV_data_66(value);
}

void MainWindow::on_horizontalSlider_light1_value_valueChanged(int value)
{
    light_->setLuminance1(ui->spinBox_light1_value->value());
}

void MainWindow::on_horizontalSlider_light2_value_valueChanged(int value)
{
    light_->setLuminance2(ui->spinBox_light2_value->value());
}

void MainWindow::on_spinBox_light1_value_valueChanged(int arg1)
{
    light_->setLuminance1(ui->spinBox_light1_value->value());
}

void MainWindow::on_spinBox_light2_value_valueChanged(int arg1)
{
    light_->setLuminance2(ui->spinBox_light2_value->value());
}


void MainWindow::on_button_delete_common_pos_clicked()
{
    int current_index = ui->tableWidget_robotarm_common_pos->currentRow();

    if(current_index == -1){
        QMessageBox::information(this,"提示\t","选中要删除行\t");
    }

    if(ui->tableWidget_robotarm_common_pos->item(current_index, 0) != nullptr){
        QString name = ui->tableWidget_robotarm_common_pos->item(current_index, 0)->text();
        mainthread_robot_arm_->remove_common_pose(name);
    }

     ui->tableWidget_robotarm_common_pos->removeRow(current_index);
}


void MainWindow::on_button_moveto_common_pos_pressed()
{
    int current_index = ui->tableWidget_robotarm_common_pos->currentRow();

    QString name = ui->tableWidget_robotarm_common_pos->item(current_index,0)->text();

    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    //判断移动方式
    if(ui->radioButton_common_line->isChecked()){
        mainthread_robot_arm_->move_to_common_pose(name,
                                                  MoveType::LINE,
                                                  LineSpeed,LineACC,LineDEC,
                                                  BlockMotion::UNLOCKED);
    }
    else{
        mainthread_robot_arm_->move_to_common_pose(name,
                                                  MoveType::JOINT,
                                                  JointSpeed,JointACC,JointDEC,
                                                  BlockMotion::UNLOCKED);
    }


}


void MainWindow::on_button_moveto_common_pos_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_button_add_station_pos_clicked()
{
    RobotPoseXYZ data = mainthread_robot_arm_->getRobotPoseXYZ();

    int countIndex = ui->tableWidget_robotarm_station_pos->rowCount();

    ui->tableWidget_robotarm_station_pos->setRowCount(countIndex + 1);
    ui->tableWidget_robotarm_station_pos->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

    ui->tableWidget_robotarm_station_pos->setItem(countIndex,1,new QTableWidgetItem(QString::number(data.x , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(countIndex,2,new QTableWidgetItem(QString::number(data.y , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(countIndex,3,new QTableWidgetItem(QString::number(data.z , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(countIndex,4,new QTableWidgetItem(QString::number(data.rx , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(countIndex,5,new QTableWidgetItem(QString::number(data.ry , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(countIndex,6,new QTableWidgetItem(QString::number(data.rz , 'f', 6 )));

    QMessageBox::information(this, "提示\t", "命名点位，然后选择保存\t");
}

void MainWindow::on_button_delete_station_pos_clicked()
{
    int current_index = ui->tableWidget_robotarm_station_pos->currentRow();

    if(current_index == -1){
        QMessageBox::information(this,"提示\t","选中要删除行\t");
    }

    if(ui->tableWidget_robotarm_station_pos->item(current_index, 0) != nullptr){
        QString name = ui->tableWidget_robotarm_station_pos->item(current_index, 0)->text();
        QString station_id = QString::number(ui->spinBox_station_id->value());

        if(!name.isEmpty()){
               mainthread_robot_arm_->remove_station_pose(station_id, name);

               QMessageBox::information(this,"提示\t","确认完全移除点位，请保存机台数据\t");
        }
    }

    ui->tableWidget_robotarm_station_pos->removeRow(current_index);
}

void MainWindow::on_button_moveto_station_pos_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_button_load_station_pos_clicked()
{
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "RobotArmPose/Station/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无点位数据\t";
            QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                QJsonValue jvalue = obj[key];
                double pose[6];
                if(jvalue.isArray()){
                    QJsonArray array = jvalue.toArray();
                    int nSize = array.size();

                    for(int i=0; i < nSize; ++i){
                        QJsonValue value = array.at(i);
                        if(value.isDouble()){
                            pose[i] = value.toDouble();
                        }
                    }
                }

                RobotPoseXYZ joint_pose ={
                    pose[0],
                    pose[1],
                    pose[2],
                    pose[3],
                    pose[4],
                    pose[5]
                };

                mainthread_robot_arm_->add_station_pose(it,key,joint_pose);

            }
        }

    }

    if(station_ids.size() != 0){
       ui->comboBox_station_name->setCurrentIndex(0);
       on_comboBox_station_name_currentIndexChanged(0);
    }

    QMessageBox::information(this, "提示\t", "加载机台数据完成\t");
}

void MainWindow::on_button_save_station_pos_clicked()
{
    //QMessageBox::information(this, "提示\t", "开始保存数据\t");

    QString DirPath = QCoreApplication::applicationDirPath();
    QString DirBaseName = "RobotArmPose";
    QString subDirName = "Station";

    mkdir_ifneed(DirPath +"/" + DirBaseName);
    mkdir_ifneed(DirPath +"/" + DirBaseName+ "/" + subDirName);

    QString station_name = ui->comboBox_station_name->currentText();
    if(station_name.isEmpty()){
        QMessageBox::information(this, "提示\t", "机台名不能为空\t");
        return;
    }

    QFile RobotPoseData(DirPath + "/"
                        + DirBaseName+ "/"
                        + subDirName + "/"
                        + station_name + ".json");

    //| QIODevice::Append
    if(!RobotPoseData.open(QIODevice::WriteOnly | QIODevice::Text ))
    {
        return;
    }

    QJsonDocument jsonDoc;
    int rows = ui->tableWidget_robotarm_station_pos->rowCount();

    QJsonObject jsonObj;
    for(int i=0; i<rows; i++){

        //record data
        bool data_available = true;
        QJsonArray array;
        for(int j=0; j<6; j++){

           double value = 0.0;
           if(ui->tableWidget_robotarm_station_pos->item(i,j+1) == NULL){
               data_available = false;
               break;
           }
           value = ui->tableWidget_robotarm_station_pos->item(i,j+1)->text().toDouble();
           array.append(value);
        }

        //record name
        bool name_available = true;
        QString name;
        if(ui->tableWidget_robotarm_station_pos->item(i,0) == NULL){
            name_available = false;
        }else{
            name = ui->tableWidget_robotarm_station_pos->item(i,0)->text();
        }

        if(!name.isEmpty()){
            jsonObj.insert(name,array);

            RobotPoseXYZ xyz_pose = { array.at(0).toDouble(),array.at(1).toDouble(),
                                          array.at(2).toDouble(),array.at(3).toDouble(),
                                          array.at(4).toDouble(),array.at(5).toDouble()};

            QString station_id = ui->comboBox_station_name->currentText();

            mainthread_robot_arm_->add_station_pose(station_id, name, xyz_pose);
        }
    }

    jsonDoc.setObject(jsonObj);
    RobotPoseData.write(jsonDoc.toJson());
    RobotPoseData.close();
    QString id = ui->comboBox_station_name->currentText();
    QString message = "机台 " + id + " 数据保存完成\t";
    QMessageBox::information(this, "提示\t", message);
}

void MainWindow::on_button_moveto_station_pos_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    QString station_id = ui->comboBox_station_name->currentText();

    int current_index = ui->tableWidget_robotarm_station_pos->currentRow();

    QString pose_name = ui->tableWidget_robotarm_station_pos->item(current_index,0)->text();


    if(ui->radioButton_station_line->isChecked()){
        mainthread_robot_arm_->move_to_station_pose(station_id,
                                                   pose_name,
                                                   MoveType::LINE,
                                                   LineSpeed,LineACC,LineDEC,
                                                   BlockMotion::UNLOCKED);
    }
    else{
        mainthread_robot_arm_->move_to_station_pose(station_id,
                                                   pose_name,
                                                   MoveType::JOINT,
                                                   JointSpeed,JointACC,JointDEC,
                                                   BlockMotion::UNLOCKED);
    }


}

void MainWindow::on_comboBox_station_name_pose_currentTextChanged(const QString &arg1)
{


}

std::set<QString> MainWindow::read_stationid_fromfile(){

    station_ids.clear();
    ui->comboBox_station_name->clear();
    ui->comboBox_station_name_matrix->clear();

    QString DirPath = QCoreApplication::applicationDirPath() + "/";
    QString DirName = "RobotArmPose/Station/";

    QString RobotPoseData_Dir = DirPath + DirName +"/station_ids.json";
    //打开文件
    QFile file(RobotPoseData_Dir);
    if(!file.exists())
    {
        QMessageBox::critical(this, "错误\t", "找不到该文件!\t");
        return station_ids;
    }
    file.open(QIODevice::ReadOnly);
    QByteArray data=file.readAll();
    //file.close();
    //使用json文件对象加载字符串
    QJsonDocument doc=QJsonDocument::fromJson(data);
    //判断是否对象
    if(doc.isObject())
    {
        //把json文档转换为json对象
        QJsonObject obj=doc.object();

        //获取key为station_ids的数组
        QJsonValue jvalue = obj["station_ids"];
        //遍历数组
        QJsonArray array = jvalue.toArray();
        int nSize = array.size();

        for(int i=0; i < nSize; ++i){
            QJsonValue value = array.at(i);
            if(value.isString()){
                QString name = value.toString();

                if(station_ids.find(name) == station_ids.end()){
                    ui->comboBox_station_name->addItem(name);
                    ui->comboBox_station_name_matrix->addItem(name);
                    ui->comboBox_station_name_qr->addItem(name);
                }

                station_ids.emplace(name);

            }
        }
     }
    return station_ids;
}

void MainWindow::write_stationid_tofile(){

    QString DirPath = QCoreApplication::applicationDirPath() + "/";
    QString DirBaseName = "RobotArmPose";
    QString subDirName = "Station";

    mkdir_ifneed(DirPath +"/" + DirBaseName);
    mkdir_ifneed(DirPath +"/" + DirBaseName+ "/" + subDirName);

    QFile StationIdData(DirPath + "/"
                        + DirBaseName+ "/"
                        + subDirName + "/"
                        + "station_ids.json");

    //| QIODevice::Append
    if(!StationIdData.open(QIODevice::WriteOnly | QIODevice::Text ))
    {
        return;
    }

    QJsonDocument jsonDoc;
    int rows = ui->tableWidget_robotarm_common_pos->rowCount();

    QJsonObject jsonObj;

    QJsonArray station_id;

    for(auto it : station_ids){
        station_id.append(it);
    }

    jsonObj.insert("station_ids", QJsonValue(station_id));

    jsonDoc.setObject(jsonObj);
    StationIdData.write(jsonDoc.toJson());
    StationIdData.close();
    //QMessageBox::information(this, "提示\t", "数据保存完成\t");
}

void MainWindow::on_pushButton_create_station_clicked()
{
    QString station_id = QString::number(ui->spinBox_station_id->value());

    if(station_ids.find(station_id) != station_ids.end())
    {
        QMessageBox::critical(this, "错误\t", "机台号已存在\t");
        return;
    }
    else{
        station_ids.emplace(station_id);
    }

    ui->comboBox_station_name->addItem(station_id);
    ui->comboBox_station_name_matrix->addItem(station_id);

    int count = ui->comboBox_station_name->count();

    Q_ASSERT(count >= 1);
    ui->comboBox_station_name->setCurrentIndex(count - 1);

    write_stationid_tofile();
    //ui->comboBox_station_name->setCurrentIndex();

}

void MainWindow::mkdir_ifneed(QString filedir){

    QDir dir(filedir);

    if (!dir.exists())
    {
        //qDebug() << "make dir is " << filedir;
        dir.mkdir(filedir);
    }
}

void MainWindow::on_comboBox_station_name_currentIndexChanged(int index)
{
    ui->tableWidget_robotarm_station_pos->setRowCount(0);

    auto station_data = mainthread_robot_arm_->getRobotStationPose();

    QString name = ui->comboBox_station_name->currentText();

    if(station_data.find(name) == station_data.end()) return;

    std::map<QString,RobotPoseXYZ> station_pose = station_data.at(name);

    for(auto it : station_pose){

        //write to table
        int countIndex = ui->tableWidget_robotarm_station_pos->rowCount();

        ui->tableWidget_robotarm_station_pos->setRowCount(countIndex + 1);
        ui->tableWidget_robotarm_station_pos->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

        ui->tableWidget_robotarm_station_pos->setItem(countIndex,0,new
                                              QTableWidgetItem(it.first));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,1,
                                              new QTableWidgetItem(QString::number(it.second.x,'f',6)));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,2,
                                              new QTableWidgetItem(QString::number(it.second.y,'f',6)));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,3,
                                              new QTableWidgetItem(QString::number(it.second.z,'f',6)));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,4,
                                              new QTableWidgetItem(QString::number(it.second.rx,'f',6)));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,5,
                                              new QTableWidgetItem(QString::number(it.second.ry,'f',6)));
        ui->tableWidget_robotarm_station_pos->setItem(countIndex,6,
                                              new QTableWidgetItem(QString::number(it.second.rz,'f',6)));
    }


    //
    ui->comboBox_station_name_matrix->setCurrentIndex(index);
}

void MainWindow::on_pushButton_save_calibration_clicked()
{
    QString DirPath = QCoreApplication::applicationDirPath();
    QString DirBaseName = "Calibration";
    QString DirSubName = "TransformMatrix";

    mkdir_ifneed(DirPath +"/" + DirBaseName);
    mkdir_ifneed(DirPath +"/" + DirBaseName + "/" + DirSubName);

    QString station_name = ui->comboBox_station_name_matrix->currentText();
    if(station_name.isEmpty()){
        QMessageBox::information(this, "提示\t", "机台名不能为空\t");
        return;
    }

    QFile calibrationData(DirPath + "/"
                        + DirBaseName + "/"
                        + DirSubName  + "/"
                        + station_name + ".json");

    //QIODevice::Append
    if(!calibrationData.open(QIODevice::WriteOnly | QIODevice::Text ))
    {
        return;
    }

    QJsonDocument jsonDoc;
    double matrixData_A = ui->doubleSpinBox_matrixData_A->value();
    double matrixData_B = ui->doubleSpinBox_matrixData_B->value();
    double matrixData_C = ui->doubleSpinBox_matrixData_C->value();
    double matrixData_D = ui->doubleSpinBox_matrixData_D->value();
    double matrixData_E = ui->doubleSpinBox_matrixData_E->value();
    double matrixData_F = ui->doubleSpinBox_matrixData_F->value();

    QJsonObject jsonObj;

    QJsonArray array;
    array.append(matrixData_A);
    array.append(matrixData_B);
    array.append(matrixData_C);
    array.append(matrixData_D);
    array.append(matrixData_E);
    array.append(matrixData_F);

    //wrap_matrix
    QString name = "wrap_matrix";
    jsonObj.insert(name, array);

    //reference_angle
    double angle = ui->doubleSpinBox_reference_value->value();
    jsonObj.insert("reference_angle",QJsonValue(angle));

    //reference_pose
    double pose_x = ui->tableWidget_base_pose->item(0,0)->text().toDouble();
    double pose_y = ui->tableWidget_base_pose->item(0,1)->text().toDouble();
    double pose_z = ui->tableWidget_base_pose->item(0,2)->text().toDouble();
    double pose_rx = ui->tableWidget_base_pose->item(0,3)->text().toDouble();
    double pose_ry = ui->tableWidget_base_pose->item(0,4)->text().toDouble();
    double pose_rz = ui->tableWidget_base_pose->item(0,5)->text().toDouble();

    QJsonArray reference_pose;
    reference_pose.append(pose_x);
    reference_pose.append(pose_y);
    reference_pose.append(pose_z);
    reference_pose.append(pose_rx);
    reference_pose.append(pose_ry);
    reference_pose.append(pose_rz);

    QString pose_name = "reference_pose";
    jsonObj.insert(pose_name, reference_pose);

    QString type_name = "type";
    QString type_info = ui->comboBox_station_type->currentText();
    jsonObj.insert(type_name, type_info);


    int exposure_time = ui->spinBox_camera_1_exposureTime->value();
    QString exposure_time_name = "exposure_time";
    jsonObj.insert(exposure_time_name,exposure_time);


    jsonDoc.setObject(jsonObj);
    calibrationData.write(jsonDoc.toJson());
    calibrationData.close();
    QString id = ui->comboBox_station_name_matrix->currentText();
    QString message = "机台 " + id + " 九点标定数据保存完成\t";
    QMessageBox::information(this, "提示\t", message);
}

void MainWindow::on_pushButton_load_calibration_clicked()
{
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "Calibration/TransformMatrix/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无转换矩阵数据\t";
            QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();
            
            for(const QString& key : obj.keys()){

                if(key == "wrap_matrix"){
                    
                    QJsonValue jvalue = obj[key];
                    double matrix[6];
                    if(jvalue.isArray()){
                        QJsonArray array = jvalue.toArray();
                        int nSize = array.size();

                        for(int i=0; i < nSize; ++i){
                            QJsonValue value = array.at(i);
                            if(value.isDouble()){
                                matrix[i] = value.toDouble();
                            }
                        }
                    }

                    cv::Mat wrap_mat(2,3,CV_32FC1);
                    wrap_mat.at<float>(0,0) = matrix[0];
                    wrap_mat.at<float>(0,1) = matrix[1];
                    wrap_mat.at<float>(0,2) = matrix[2];
                    wrap_mat.at<float>(1,0) = matrix[3];
                    wrap_mat.at<float>(1,1) = matrix[4];
                    wrap_mat.at<float>(1,2) = matrix[5];

                    calibration_.add_station_transform_matrix(it, wrap_mat);
                    
                }

                if(key == "reference_pose"){

                    QJsonValue jvalue = obj[key];
                    double pose[6];
                    if(jvalue.isArray()){
                        QJsonArray array = jvalue.toArray();
                        int nSize = array.size();

                        for(int i=0; i < nSize; ++i){
                            QJsonValue value = array.at(i);
                            if(value.isDouble()){
                                pose[i] = value.toDouble();
                            }
                        }
                    }

                    RobotPoseXYZ reference_pose = {
                        pose[0], pose[1], pose[2],
                        pose[3], pose[4], pose[5]
                    };

                    calibration_.add_station_reference_pose(it, reference_pose);
                }

                if(key == "reference_angle"){
                     QJsonValue jvalue = obj[key];

                     if(jvalue.isDouble()){
                         double angle = jvalue.toDouble();
                         calibration_.add_station_reference_angle(it, angle);
                     }
                }

                if(key == "type"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isString()){
                        QString type = jvalue.toString();
                        calibration_.add_station_type(it, type);
                    }
                }

                if(key == "exposure_time"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isDouble()){
                        int value = jvalue.toDouble();
                        calibration_.add_station_exposure_time(it, value);
                    }
                }

            }
        }

    }

    if(station_ids.size() != 0){
        ui->comboBox_station_name_matrix->setCurrentIndex(0);
        on_comboBox_station_name_matrix_currentIndexChanged(0);
    }

    QMessageBox::information(this, "提示\t", "加载机台标定数据完成\t");
}

void MainWindow::on_comboBox_station_name_matrix_currentIndexChanged(int index)
{
    auto station_matrix_data = calibration_.get_station_transform_matrix();

    QString name = ui->comboBox_station_name_matrix->currentText();

    //当前使用机台
    current_station_id = name;

    if(station_matrix_data.find(name) == station_matrix_data.end()){

//        QString message = "机台 " + name + " 转换矩阵未知\t";
//        QMessageBox::information(this, "提示\t", message);
       return;
    }

    cv::Mat matrix_data = station_matrix_data.at(name);

    double matrixData_A = matrix_data.at<float>(0,0);
    double matrixData_B = matrix_data.at<float>(0,1);
    double matrixData_C = matrix_data.at<float>(0,2);
    double matrixData_D = matrix_data.at<float>(1,0);
    double matrixData_E = matrix_data.at<float>(1,1);
    double matrixData_F = matrix_data.at<float>(1,2);

    ui->doubleSpinBox_matrixData_A->setValue(matrixData_A);
    ui->doubleSpinBox_matrixData_B->setValue(matrixData_B);
    ui->doubleSpinBox_matrixData_C->setValue(matrixData_C);
    ui->doubleSpinBox_matrixData_D->setValue(matrixData_D);
    ui->doubleSpinBox_matrixData_E->setValue(matrixData_E);
    ui->doubleSpinBox_matrixData_F->setValue(matrixData_F);


    //加载机台参考角度
    auto station_reference_data = calibration_.get_station_reference_angle();

    if(station_reference_data.find(name)!= station_reference_data.end()) {

        float angle_data = station_reference_data.at(name);

        ui->doubleSpinBox_reference_value->setValue(angle_data);

    }


    //加载机台参考位姿
    auto station_reference_pose = calibration_.get_station_reference_pose();

    if(station_reference_pose.find(name) != station_reference_pose.end()){
        RobotPoseXYZ base_pose = station_reference_pose.at(name);

        ui->tableWidget_reference_pose->setRowCount(0);
        int countIndex = ui->tableWidget_reference_pose->rowCount();
        ui->tableWidget_reference_pose->setRowCount(countIndex + 1);
        ui->tableWidget_reference_pose->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

        ui->tableWidget_reference_pose->setItem(countIndex,0,new QTableWidgetItem(QString::number(base_pose.x , 'f', 6 )));
        ui->tableWidget_reference_pose->setItem(countIndex,1,new QTableWidgetItem(QString::number(base_pose.y , 'f', 6 )));
        ui->tableWidget_reference_pose->setItem(countIndex,2,new QTableWidgetItem(QString::number(base_pose.z , 'f', 6 )));
        ui->tableWidget_reference_pose->setItem(countIndex,3,new QTableWidgetItem(QString::number(base_pose.rx , 'f', 6 )));
        ui->tableWidget_reference_pose->setItem(countIndex,4,new QTableWidgetItem(QString::number(base_pose.ry , 'f', 6 )));
        ui->tableWidget_reference_pose->setItem(countIndex,5,new QTableWidgetItem(QString::number(base_pose.rz , 'f', 6 )));

    }

    //加载机台类型
    auto station_type = calibration_.get_station_type();
    if(station_type.find(name) != station_type.end()){
        QString type = station_type.at(name);
        ui->comboBox_station_type->setCurrentText(type);
    }


    int value = calibration_.get_station_exposure_time(name);
    ui->spinBox_camera_1_exposureTime->setValue(value);


    //设置机台点位
    //int index = ui->comboBox_station_name_matrix->currentIndex();
    ui->comboBox_station_name->setCurrentIndex(index);


}

void MainWindow::on_horizontalSlider_exposure_time_valueChanged(int value)
{
    ui->spinBox_camera_1_exposureTime->setValue(value);
    camera_->set_exposure_time(value);
}


void MainWindow::on_pushButton_make_datamtraix_clicked()
{
    QString str =ui->textEdit->toPlainText();
    if(str.isEmpty())
    {
        QMessageBox::information(this,"信息","没有输入文本");
    }
    else
    {
        QString str2=str;
        //qDebug()<<"字节长度："<<str2.size();
        // QRegExp rx("^[0-9]*$");//匹配数字
        QRegExp rx("^\\D*$");//匹配非数字
        QRegExp rx2("^[u4e00-u9fa5]*$");//匹配汉字[u4e00-u9fa5]
        int strL=str2.size();
        for(int i=0; i<str2.size(); i++)
        {
            if(rx.exactMatch(str2.at(i)))
            {
                //qDebug()<<"匹配非数字："<<str2.at(i);
                strL++;
            }

            if(rx2.indexIn(str2.at(i)))
            {
                //qDebug()<<"匹配汉字："<<str2.at(i);
                strL=strL+4;
            }
        }
        //qDebug()<<"字节复核长度："<<strL;

        int flagPass=1;
        if(ui->comboBox_data_matrix->currentData().toInt()>=0)
        {
            int errSize=ui->comboBox_data_matrix->currentData().toInt()+1;
            //qDebug()<<"errSize："<<errSize;
            if(errSize==1&&strL>6){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==2&&strL>10){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==3&&strL>16){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==4&&strL>24){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==5&&strL>36){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==6&&strL>44){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==7&&strL>60){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==8&&strL>60){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==9&&strL>88){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==10&&strL>124){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==11&&strL>172){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==12&&strL>228){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==13&&strL>288){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==14&&strL>348){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==15&&strL>408){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==16&&strL>560){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==17&&strL>736){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==18&&strL>912){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==19&&strL>1152){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==20&&strL>1392){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==21&&strL>1632){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==22&&strL>2100){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==23&&strL>2608){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==24&&strL>3116){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}

            if(errSize==25&&strL>10){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==26&&strL>20){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==27&&strL>32){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==28&&strL>44){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==29&&strL>64){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}
            if(errSize==30&&strL>98){QMessageBox::information(this,"信息","Hyperbyte cannot be created");flagPass=0;}

            //qDebug()<<"flagPass："<<flagPass;
        }

        if(flagPass>0)
        {
            DmtxEncode* encode = dmtxEncodeCreate();
            Q_ASSERT(encode != NULL);

            encode->moduleSize =ui->spinBox_moduleSize->value();
            encode->marginSize =ui->spinBox_marginSize->value();

            //        QString comstr=ui->comboBox->currentText();
            //qDebug()<<"comboBox: "<<ui->comboBox_data_matrix->currentData().toInt();

            encode->sizeIdxRequest=ui->comboBox_data_matrix->currentData().toInt();//DmtxSymbolSquareAuto;//设置类型默认

            //    dmtxEncodeSetProp( encode, DmtxPropModuleSize, 5); // encode->moduleSize = 5;
            //    dmtxEncodeSetProp( encode, DmtxPropMarginSize,20); //  encode->marginSize = 10;
            //    //下面是代码中默认函数设置 dmtxencode.c->dmtxEncodeCreate(void);
            //    encode->scheme = DmtxSchemeAscii;
            //    encode->sizeIdxRequest = DmtxSymbolSquareAuto;
            //    encode->marginSize = 10;
            //    encode->moduleSize = 5;
            //    encode->pixelPacking = DmtxPack24bppRGB;
            //    encode->imageFlip = DmtxFlipNone;
            //    encode->rowPadBytes = 0;
            //    encode->fnc1 = DmtxUndefined;

            //    int ret = dmtxEncodeDataMatrix(encode, strlen(str.toStdString().c_str()), (unsigned char*)str.toStdString().c_str());//案例函数c语言
            int ret = dmtxEncodeDataMatrix(encode, str.size(), (uchar*)str.toStdString().data());
            Q_ASSERT(ret == 1);

            //    QMessageBox::information(this,"信息","打开文件失败");
            //str.toStdString().c_str()//兼容c语言

            int width = dmtxImageGetProp(encode->image, DmtxPropWidth);
            int height = dmtxImageGetProp(encode->image, DmtxPropHeight);
            int bytesPerPixel = dmtxImageGetProp(encode->image, DmtxPropBytesPerPixel);
            int bytesPerLine = dmtxImageGetProp(encode->image, DmtxPropRowSizeBytes);

            uchar *pxlData = (uchar *)malloc(width*height*bytesPerPixel);
            memcpy(pxlData,encode->image->pxl,width*height*bytesPerPixel);
            dmtxEncodeDestroy(&encode);

//            qDebug()<<"宽度: "<<width;
//            qDebug()<<"高度: "<<height;
//            qDebug()<<"比特: "<<bytesPerPixel;
//            qDebug()<<"行数: "<<bytesPerLine;

            QImage img = QImage(pxlData,width,height,bytesPerLine,QImage::Format_RGB888);

//            qDebug()<<"QImage 宽度: "<<img.width();
//            qDebug()<<"QImage 高度: "<<img.height();
//            qDebug()<<"QImage 行数: "<<img.bytesPerLine();


            QImage imgshow=img.scaled( this->ui->label_data_matrix->width(), this->ui->label_data_matrix->height(),Qt:: KeepAspectRatio);
            ui->label_data_matrix->setPixmap(QPixmap::fromImage(imgshow));

            QString base_str=str.left(8);
            QString label_base_text = QString("基本信息: ")
                    + QString("宽度 ") + QString::number(width) + QString(" ")
                    + QString("高度 ") + QString::number(height) + QString(" ")
                    + QString("字节 ") + QString::number(strL) + QString(" ")
                    + QString("内容 ") + base_str;
            ui->label_base->setText(label_base_text);
            globalImg=img.copy();//保存用
            globastr=QString("D%1-W%2-L%3").
                    arg(base_str).arg(width).arg(height);
        }
    }
}

void MainWindow::on_pushButton_save_datamatrix_clicked()
{
    QImage images=globalImg;
    QString dirPath;//创建文件夹路径
    QString filePath;//存储保存路径
    QDateTime time = QDateTime::currentDateTime();//获取系统现在的时间
    QString str = time.toString("yyyy-MM-dd-hhmmss"); //设置显示格式
    //      QString str = time.toString("yyyyMMdd_hhmmss"); //设置显示格式
    QString saveName=QString("%1-%2").arg(globastr).arg(str);//保存图片名字

    if(images.isNull())
    {
        QMessageBox::information(this,"信息","no image");
    }
    else
    {
        filePath = QFileDialog::getExistingDirectory(this,"");//获取文件夹路径
        if(filePath.isEmpty())
        {
            QMessageBox::information(this,"信息","保存失败");
        }
        else
        {
            //         dirPath=QString("%1/保存实验%2").arg(filePath).arg(str);
            //         QDir *temp = new QDir;//声明文件对象
            //         temp->mkdir(dirPath);//创建文件夹
            //将图片重新命名并保存至刚刚创建的文件夹里
            QString savePath=QString("%1//%2.jpg").arg(filePath).arg(saveName);
            images.save(savePath);//QImage保存方法
            QMessageBox::information(this,"信息","保存成功");
        }
    }
}

void MainWindow::on_pushButton_read_image_clicked()
{
    ui->label_data_matrix->clear();//先清除label内容
    //打开图片操作
    QString fileName = QFileDialog::getOpenFileName(
                this, tr("open image file"),
                "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)"));

    if(fileName.isEmpty())
    {
        QMessageBox mesg;
        mesg.warning(this,"警告","打开图片失败!");
        return;
    }
    else
    {
        ui->label_data_matrix->clear();
        QPixmap pix(fileName);
        pix=pix.scaled(ui->label_data_matrix->width(),ui->label_data_matrix->height(),Qt::KeepAspectRatio);
        ui->label_data_matrix->setPixmap(pix);
        ui->textEdit->clear();

        QImage img(fileName);
        // QImage imgcp=img.scaled(ui->label->width(),ui->label->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
        // ui->label->setPixmap(QPixmap::fromImage(imgcp));//QImage 显示速度慢
        globalImg2=img.copy();

        Dm_image = cv::imread(fileName.toStdString());

        //cv::imshow("aaa",Dm_image);
        //cv::waitKey(1);

        //qDebug()<<"src.format() "<<img.format();

    }


}

void MainWindow::on_pushButton_decode_datamatrix_clicked()
{

    //ROI
    cv::Rect rect(876,172,1200,1200);
    cv::Mat Dm_image_roi = Dm_image(rect);

    //cv::imshow("ROI",Dm_image_roi);
    //cv::waitKey(1);

    //return;

    //Resize
    cv::Size size(Dm_image_roi.rows/2 ,Dm_image_roi.cols/2);
    cv::Mat resize_image;
    cv::resize(Dm_image_roi,resize_image,size);

    //cv::imshow("Resize",resize_image);
    //cv::waitKey(1);
    //return;

    //平滑
    cv::Mat blur_mat;
    cv::medianBlur(resize_image,blur_mat,5);
    //cv::imshow("blur_mat",blur_mat);
    //cv::waitKey(1);

    //二值化
    //cv::Mat threshold_image;
    //cv::threshold(resize_image,threshold_image,100,255,0);
    //cv::imshow("threshold",threshold_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/threshold.bmp",threshold_image);

    cv::Mat open_image;
    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));
    cv::morphologyEx(blur_mat,open_image,cv::MORPH_OPEN,element);
    //cv::imshow("open",open_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/open.bmp",open_image);

    cv::Mat dilate_image;
    cv::Mat element2 = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3));
    cv::morphologyEx(open_image,dilate_image,cv::MORPH_ERODE,element2);
    //cv::imshow("dilate",dilate_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/dilate.bmp",dilate_image);


    //转换为QImage
    QImage qcolor = QImage((const unsigned char*)(dilate_image.data),
                           dilate_image.cols, dilate_image.rows, dilate_image.step,
                           QImage::Format_RGB888);


    QTime time;
    time.start();
    QImage img = qcolor;                 //globalImg2;
    if(img.isNull())
    {
        QMessageBox::information(this,"信息","打开文件失败");
    }
    else
    {
        DmtxMessage *msg;
        DmtxRegion *reg;
        QImage src =img.copy();
        DmtxImage *imgdtx;
        QPixmap pix=QPixmap::fromImage(src);
        QString str;
        int leftLineAngle;
        int bottomLineAngle;
        int cornerx;
        int cornery;

        //qDebug()<<"src.format() "<<src.format();

        if(src.format()==QImage::Format_Mono)
        {
        src=src.convertToFormat(QImage::Format_ARGB32);
        //qDebug()<<"转换后：src.format() "<<src.format();
        }

        int pack=DmtxPack32bppXRGB;
        switch(src.format())
            {
            case QImage::Format_ARGB32:
            pack=DmtxPack32bppXRGB;
            break;
            case QImage::Format_RGB32:
            pack=DmtxPack32bppXRGB;
            break;
            case QImage::Format_RGB888:
            pack=DmtxPack24bppRGB;
            break;
            case QImage::Format_Indexed8:
            pack=DmtxPack8bppK;
            break;
            case  QImage::Format_Grayscale8:
            pack=DmtxPack8bppK;
            }

        // 增加超时时间。
        DmtxTime beginTime = dmtxTimeNow();	// 根据系统设置情况，获得当前时间
        long timeout_ms = 2000;
        DmtxTime stopTime = dmtxTimeAdd(beginTime, timeout_ms);	// 增加xx ms

        imgdtx = dmtxImageCreate(src.bits(),src.width(),src.height(),pack);
        Q_ASSERT(imgdtx != NULL);

        DmtxDecode *dec = dmtxDecodeCreate(imgdtx, 1);
        Q_ASSERT(dec != NULL);

        reg = dmtxRegionFindNext(dec,&stopTime);	// 如果超时则认为没有找到
        // assert(reg != NULL);

        if(dmtxTimeExceeded(stopTime))
        {
             //qDebug()<<"超时";
             QMessageBox::information(this,"信息","decode timeout");
        }
       else
        {
            if (reg != NULL)
            {
                msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
                if (msg != NULL)
                {
                    //                    cout << msg->output << endl;
                    //                    cout << msg->outputIdx << endl;

                    std::string strout = (char*)msg->output;//解码信息
                    str=QString::fromStdString(strout);
                    QString outmsg=QString("<font color=\"#00FF00\">%1</font>").arg(QString::fromStdString(strout));
                    ui->textEdit->setText(outmsg);

                    //  qDebug()<<"reg->leftAngle "<<reg->leftAngle;
                    //  qDebug()<<"reg->leftAngle "<<reg->bottomAngle;
                    //   qDebug()<<"reg->finalPos.X "<<reg->finalPos.X;
                    //   qDebug()<<"reg->finalPos.Y "<<reg->finalPos.Y;
                    //qDebug()<<"reg->leftLine.mag"<<reg->leftLine.mag;
                    //qDebug()<<"reg->leftLine.hOffset"<<reg->leftLine.hOffset;
                    //qDebug()<<"reg->leftLine.stepBeg"<<reg->leftLine.stepBeg;
                    //qDebug()<<"reg->leftLine.stepPos"<<reg->leftLine.stepPos;
                    //qDebug()<<"reg->leftLine.stepNeg"<<reg->leftLine.stepNeg;
                    //qDebug()<<"reg->leftLine.distSq"<<reg->leftLine.distSq;
                    //qDebug()<<"reg->leftLine.locBeg.X "<<reg->leftLine.locBeg.X;
                    //qDebug()<<"reg->leftLine.locBeg.Y "<<reg->leftLine.locBeg.Y;

//                    qDebug()<<"reg->leftLine.locPos.X "<<reg->leftLine.locPos.X;//准的
//                    qDebug()<<"reg->leftLine.locPos.Y "<<reg->leftLine.locPos.Y;//准的
//                    qDebug()<<"reg->leftLine.locNeg.X "<<reg->leftLine.locNeg.X;//准的
//                    qDebug()<<"reg->leftLine.locNeg.Y "<<reg->leftLine.locNeg.Y;//准的
//                    qDebug()<<"bottomLine.locPos.X "<<reg->bottomLine.locPos.X;
//                    qDebug()<<"bottomLine.locPos.Y "<<reg->bottomLine.locPos.Y;
//                    qDebug()<<"bottomLine.locNeg.X "<<reg->bottomLine.locNeg.X;
//                    qDebug()<<"bottomLine.locNeg.y "<<reg->bottomLine.locNeg.Y;

                    QPainter p;
                    p.begin(&pix);
                    p.setPen(QPen(Qt::blue, 2));
                    p.drawEllipse(reg->leftLine.locPos.X-5,pix.height()-reg->leftLine.locPos.Y-5, 10, 10);//准的
                    p.setPen(QPen(Qt::yellow, 2));
                    p.drawEllipse(reg->leftLine.locNeg.X-5,img.height()-reg->leftLine.locNeg.Y-5, 10, 10);//准的

                    p.setPen(QPen(Qt::darkRed, 2));
                    p.drawEllipse(reg->bottomLine.locPos.X-5,img.height()-reg->bottomLine.locPos.Y-5, 10, 10);//准的
                    p.setPen(QPen(Qt::darkGreen, 2));
                    p.drawEllipse(reg->bottomLine.locNeg.X-5,img.height()-reg->bottomLine.locNeg.Y-5, 10, 10);//准的

                    int lineOriginX=(reg->leftLine.locNeg.X+reg->bottomLine.locPos.X)/2;
                    int lineOriginY=img.height()-(reg->leftLine.locNeg.Y+reg->bottomLine.locPos.Y)/2;
                    int lineLeftX=reg->leftLine.locPos.X;
                    int lineLeftY=pix.height()-reg->leftLine.locPos.Y;
                    int lineBottomX=reg->bottomLine.locNeg.X;
                    int lineBottomY=img.height()-reg->bottomLine.locNeg.Y;

                    p.setPen(QPen(Qt::red, 2));
                    p.drawLine(lineOriginX,lineOriginY,lineLeftX,lineLeftY);
                    p.drawLine(lineOriginX,lineOriginY,lineBottomX,lineBottomY);
                    p.end();

                    leftLineAngle=reg->leftLine.angle;
                    bottomLineAngle=reg->bottomAngle;
                    cornerx=lineOriginX;
                    cornery=lineOriginY;

                    dmtxMessageDestroy(&msg);
                }
                else
                {
                    //qDebug()<<"无法检测到2";
                    QMessageBox::information(this,"信息","无法检测到2");
                }
                dmtxRegionDestroy(&reg);
            }
            else
            {
                //qDebug()<<"无法检测到1";
                QMessageBox::information(this,"信息","无法检测到1");
            }

        }

        dmtxDecodeDestroy(&dec);
        dmtxImageDestroy(&imgdtx);

        //qDebug()<<time.elapsed()/1000.0<<"s";

        ui->label_data_matrix->clear();
//        src=src.scaled(ui->label->width(),ui->label->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
//        ui->label->setPixmap(QPixmap::fromImage(src));//QImage 显示速度慢
        pix=pix.scaled(ui->label_data_matrix->width(),ui->label_data_matrix->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
        ui->label_data_matrix->setPixmap(pix);

        ui->label_base->clear();
        QString base_str=str.left(8);
        QString label_base_text=QString("信息:左边 %1°底边 %2°角点X %3 角点Y %4 内容 %5..").
                arg(leftLineAngle).arg(bottomLineAngle).arg(cornerx).arg(cornery).arg(base_str);
        ui->label_base->setText(label_base_text);

        QString timestr= QString("解码耗时 ")
                        + QString::number(time.elapsed())
                        + QString(" ms");
        ui->label_time->setText(timestr);
    }

    //dataMatrixDecode();//简化版本
}


void MainWindow::Initial_encoding_format(){
    ui->comboBox_data_matrix->addItem("DmtxSymbolRectAuto",DmtxSymbolRectAuto);
    ui->comboBox_data_matrix->addItem("DmtxSymbolSquareAuto",DmtxSymbolSquareAuto);
    //  ui->comboBox->addItem("DmtxSymbolShapeAuto",DmtxSymbolShapeAuto);//出错
    ui->comboBox_data_matrix->addItem("DmtxSymbol10x10",DmtxSymbol10x10);
    ui->comboBox_data_matrix->addItem("DmtxSymbol12x12",DmtxSymbol12x12);
    ui->comboBox_data_matrix->addItem("DmtxSymbol14x14",DmtxSymbol14x14);
    ui->comboBox_data_matrix->addItem("DmtxSymbol16x16",DmtxSymbol16x16);
    ui->comboBox_data_matrix->addItem("DmtxSymbol18x18",DmtxSymbol18x18);
    ui->comboBox_data_matrix->addItem("DmtxSymbol20x20",DmtxSymbol20x20);
    ui->comboBox_data_matrix->addItem("DmtxSymbol22x22",DmtxSymbol22x22);
    ui->comboBox_data_matrix->addItem("DmtxSymbol24x24",DmtxSymbol24x24);
    ui->comboBox_data_matrix->addItem("DmtxSymbol26x26",DmtxSymbol26x26);
    ui->comboBox_data_matrix->addItem("DmtxSymbol32x32",DmtxSymbol32x32);
    ui->comboBox_data_matrix->addItem("DmtxSymbol36x36",DmtxSymbol36x36);
    ui->comboBox_data_matrix->addItem("DmtxSymbol40x40",DmtxSymbol40x40);
    ui->comboBox_data_matrix->addItem("DmtxSymbol44x44",DmtxSymbol44x44);
    ui->comboBox_data_matrix->addItem("DmtxSymbol48x48",DmtxSymbol48x48);
    ui->comboBox_data_matrix->addItem("DmtxSymbol52x52",DmtxSymbol52x52);
    ui->comboBox_data_matrix->addItem("DmtxSymbol64x64",DmtxSymbol64x64);
    ui->comboBox_data_matrix->addItem("DmtxSymbol72x72",DmtxSymbol72x72);
    ui->comboBox_data_matrix->addItem("DmtxSymbol80x80",DmtxSymbol80x80);
    ui->comboBox_data_matrix->addItem("DmtxSymbol88x88",DmtxSymbol88x88);
    ui->comboBox_data_matrix->addItem("DmtxSymbol96x96",DmtxSymbol96x96);
    ui->comboBox_data_matrix->addItem("DmtxSymbol104x104",DmtxSymbol104x104);
    ui->comboBox_data_matrix->addItem("DmtxSymbol120x120",DmtxSymbol120x120);
    ui->comboBox_data_matrix->addItem("DmtxSymbol132x132",DmtxSymbol132x132);
    ui->comboBox_data_matrix->addItem("DmtxSymbol144x144",DmtxSymbol144x144);

    ui->comboBox_data_matrix->addItem("DmtxSymbol8x18",DmtxSymbol8x18);//10
    ui->comboBox_data_matrix->addItem("DmtxSymbol8x32",DmtxSymbol8x32);//20
    ui->comboBox_data_matrix->addItem("DmtxSymbol12x26",DmtxSymbol12x26);//32
    ui->comboBox_data_matrix->addItem("DmtxSymbol16x36",DmtxSymbol16x36);//64
    ui->comboBox_data_matrix->addItem("DmtxSymbol16x48",DmtxSymbol16x48);//98
    ui->comboBox_data_matrix->setCurrentIndex(1);
}

void MainWindow::dataMatrixDecode(){

    QTime time;
    QImage src("D:/code/QtLearning/dmtxMaker/1.jpg");
    DmtxMessage *msg;
    DmtxRegion *reg;
    DmtxImage *imgdtx;
    QString outstr;

    time.start();//qt开始计时

    //qDebug()<<"src.format() "<<src.format();//注意图片格式要对应 dmtxImageCreate的 不然无法解码

    // 增加超时时间。
    DmtxTime beginTime = dmtxTimeNow();	// 根据系统设置情况，获得当前时间
    long timeout_ms = 200;
    DmtxTime stopTime = dmtxTimeAdd(beginTime, timeout_ms);	// 增加xx ms

    //创建dmtxImage，将qt读取到的图片存储到dmtxImage
    imgdtx = dmtxImageCreate(src.bits(),src.width(),src.height(),DmtxPack32bppXRGB);
    //Q_ASSERT(imgdtx != NULL);
    DmtxDecode *dec = dmtxDecodeCreate(imgdtx, 1);//解码
    //Q_ASSERT(dec != NULL);
    reg = dmtxRegionFindNext(dec,&stopTime);//查找下一个，如果超时则认为没有找到
    //Q_ASSERT(reg != NULL);

    if(dmtxTimeExceeded(stopTime))
    {
        //qDebug()<<"超时";
    }
    else
    {
        if (reg != NULL)
        {
            msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);//获取解码信息
            if (msg != NULL)
            {
                std::cout << msg->output << endl;//输出解码信息
                std::cout << msg->outputIdx << endl;

                //qt输出解码信息string->QString
                std::string strout = (char*)msg->output;//解码信息
                outstr=QString::fromStdString(strout);
                //qDebug()<<"解码信息:"<<outstr;//解码信息
                //qDebug()<<"解码信息xx:"<<msg->output;//解码信息

                //二维码坐标信息
                //qDebug()<<"reg->leftLine.locPos.X "<<reg->leftLine.locPos.X;
                //qDebug()<<"reg->leftLine.locPos.Y "<<reg->leftLine.locPos.Y;
//                qDebug()<<"reg->leftLine.locNeg.X "<<reg->leftLine.locNeg.X;
//                qDebug()<<"reg->leftLine.locNeg.Y "<<reg->leftLine.locNeg.Y;
//                qDebug()<<"bottomLine.locPos.X "<<reg->bottomLine.locPos.X;
//                qDebug()<<"bottomLine.locPos.Y "<<reg->bottomLine.locPos.Y;
//                qDebug()<<"bottomLine.locNeg.X "<<reg->bottomLine.locNeg.X;
//                qDebug()<<"bottomLine.locNeg.y "<<reg->bottomLine.locNeg.Y;

                dmtxMessageDestroy(&msg);
            }
            else{
                //qDebug()<<"无法检测到2";
            }

            dmtxRegionDestroy(&reg);
        }
        else{
            //qDebug()<<"无法检测到1";
        }
    }

    dmtxDecodeDestroy(&dec);
    dmtxImageDestroy(&imgdtx);
    //qDebug()<<time.elapsed()/1000.0<<"s";

}

void MainWindow::on_pushButton_load_picture_2_clicked()
{

}

void MainWindow::on_pushButton_load_picture_clicked()
{
    ui->label_show->clear();//先清除label内容
    //打开图片操作
    QString fileName = QFileDialog::getOpenFileName(
                this, tr("open image file"),
                "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)"));

    if(fileName.isEmpty())
    {
        QMessageBox mesg;
        mesg.warning(this,"警告","打开图片失败!");
        return;
    }
    else
    {
        ui->label_show->clear();
        //QPixmap pix(fileName);
        //pix=pix.scaled(ui->label_show->width(),ui->label_show->height(),Qt::KeepAspectRatio);
        //ui->label_show->setPixmap(pix);
        //ui->textEdit->clear();

        QR_image = cv::imread(fileName.toStdString());
        //QImage img(fileName);
        // QImage imgcp=img.scaled(ui->label->width(),ui->label->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
        // ui->label->setPixmap(QPixmap::fromImage(imgcp));//QImage 显示速度慢
        //globalImg2=img.copy();
        //qDebug()<<"src.format() "<<img.format();

        if(QR_image.empty()){
            //qDebug() << "QR_image is empty";
        }else{
            //cv::imshow("aaa",QR_image);
            //cv::waitKey(10);
        }


        QImage qcolor = QImage((const unsigned char*)(QR_image.data),
                               QR_image.cols, QR_image.rows, QR_image.step,
                               QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        qcolorshow = qcolor.scaled(QR_image.cols, QR_image.rows).scaled(width, height,
                                                                         Qt::IgnoreAspectRatio,
                                                                         Qt::SmoothTransformation);

        this->update();


    }


}

void MainWindow::on_pushButton_detect_QR_clicked()
{
    std::vector<cv::Point2d> corners;
    cv::Point2d center;
    float angle;

    //cv::Mat dst;
    //cv::threshold(QR_image,dst,200,255,cv::THRESH_BINARY_INV);

    //cv::imshow("dst",dst);
    //cv::waitKey(1);

    bool state = image_processor_.detectQRLandmark(QR_image,
                                      corners,
                                      center,
                                      angle);

    if(state == true)
    {
        cv::Mat QTmat = QR_image.clone();
        //cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

        //中间回字起步，然后顺时针得到4个点， 红色
        cv::circle(QTmat, corners[0], 3, cv::Scalar(255,0,0),5,-1); //第五个参数我设为-1，表明这是个实点。
        //右边回字，绿色
        cv::circle(QTmat, corners[1], 3, cv::Scalar(0,255,0),5,-1); //第五个参数我设为-1，表明这是个实点。
        //非回字，蓝色
        cv::circle(QTmat, corners[2], 3, cv::Scalar(0,0,255),5,-1); //第五个参数我设为-1，表明这是个实点。
        //左边回字，白色
        cv::circle(QTmat, corners[3], 3, cv::Scalar(255,255,255),5,-1); //第五个参数我设为-1，表明这是个实点。

        cv::circle(QTmat, center, 3, cv::Scalar(0,255,255),5,-1); //第五个参数我设为-1，表明这是个实点。

        //cv::imwrite("C:/Users/OKAGV/Desktop/QR.png",QTmat);
        //cv::waitKey(1);

        QImage qcolor = QImage((const unsigned char*)(QTmat.data), QTmat.cols, QTmat.rows, QTmat.step, QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        QImage qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
        ui->label_show->setPixmap(QPixmap::fromImage(qcolorshow));
        ui->label_show->resize(qcolorshow.size());

        QMessageBox::information(this, "提示\t", "识别到有效二维码\t");
    }
    else{
         QMessageBox::information(this, "提示\t", "未识别到有效二维码\t");
    }
}

void MainWindow::on_pushButton_moveto_QR_clicked()
{

}

void MainWindow::on_pushButton_load_reference_value_clicked()
{
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "Calibration/ReferenceAngle/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无点位数据\t";
            QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            float reference_angle;
            for(const QString& key : obj.keys()){

                if(key == "reference_angle"){

                    QJsonValue jvalue = obj[key];
                    double matrix[6];
                    if(jvalue.isDouble()){
                        reference_angle = jvalue.toDouble();
                    }

                    calibration_.add_station_reference_angle(it,reference_angle);

                }

            }
        }

    }

    if(station_ids.size() != 0){
        ui->comboBox_station_name_matrix->setCurrentIndex(0);
        on_comboBox_station_name_matrix_currentIndexChanged(0);
    }

    QMessageBox::information(this, "提示\t", "加载机台参考角度数据完成\t");
}

void MainWindow::on_pushButton_save_reference_value_clicked()
{
    QString DirPath = QCoreApplication::applicationDirPath();
    QString DirBaseName = "Calibration";
    QString DirSubName = "ReferenceAngle";

    mkdir_ifneed(DirPath +"/" + DirBaseName);
    mkdir_ifneed(DirPath +"/" + DirBaseName + "/" + DirSubName);

    QString station_name = ui->comboBox_station_name_matrix->currentText();
    if(station_name.isEmpty()){
        QMessageBox::information(this, "提示\t", "机台名不能为空\t");
        return;
    }

    QFile calibrationData(DirPath + "/"
                        + DirBaseName + "/"
                        + DirSubName  + "/"
                        + station_name + ".json");

    //QIODevice::Append
    if(!calibrationData.open(QIODevice::WriteOnly | QIODevice::Text ))
    {
        return;
    }

    QJsonDocument jsonDoc;
    float reference_angle ;//= ui->doubleSpinBox_reference_value->value();

    QJsonObject jsonObj;

    QString name = "reference_angle";
    jsonObj.insert(name, reference_angle);

    calibration_.add_station_reference_angle(station_name, reference_angle);

    jsonDoc.setObject(jsonObj);
    calibrationData.write(jsonDoc.toJson());
    calibrationData.close();
    QString id = ui->comboBox_station_name_matrix->currentText();
    QString message = "机台 " + id + " 参考角度数据保存完成\t";
    QMessageBox::information(this, "提示\t", message);
}

void MainWindow::on_stopFlowButton_clicked()
{
    //机械臂先停下来
    qDebug() << "机械臂停止运动...";
    mainthread_robot_arm_->stop_movement();

    //设置流程结束标志
    qDebug() << "设置流程停止检测标志位为false";
    work_flag = false;

    //清除所有路点
    qDebug() << "清除机械臂所有增加路点";
    mainthread_robot_arm_->clear_waypoints();

    //机械臂掉使能
    qDebug() << "软件发送机械臂掉使能指令，执行软急停";
    mainthread_robot_arm_->set_servo_status(false);

    //work_flow_->stop_work_flow();
    timer_work_flow_->stop();

    ui->startFlowButton->setEnabled(true);

    ui->groupBox_Test1->setEnabled(true);
    ui->groupBox_Test2->setEnabled(true);
    ui->groupBox_Test3->setEnabled(true);

    for(int i=1; i<13; i++){
        test3_qlabel[i]->setStyleSheet("background-color : rgb(255,255,255)");
    }

    ui->tab->setEnabled(true);
    ui->tab_1->setEnabled(true);
    ui->tab_2->setEnabled(true);
    ui->tab_3->setEnabled(true);
    ui->tab_4->setEnabled(true);
    ui->tab_5->setEnabled(true);
    ui->tab_6->setEnabled(true);
    ui->tab_7->setEnabled(true);
    //ui->tab_8->setDisabled(true);
    ui->tab_9->setEnabled(true);
    ui->tab_10->setEnabled(true);

    ui->button_load_all_station_data->setEnabled(true);

}

void MainWindow::on_spinBox_speed_valueChanged(int arg1)
{
    mainthread_robot_arm_->set_arm_speed(arg1);

    LineSpeed = arg1;
}

void MainWindow::on_spinBox_acceleration_valueChanged(int arg1)
{
    mainthread_robot_arm_->set_arm_acc(arg1);
    LineACC = arg1;

}

void MainWindow::on_spinBox_deceleration_valueChanged(int arg1)
{
    mainthread_robot_arm_->set_arm_dec(arg1);
    LineDEC = arg1;
}

void MainWindow::on_button_update_common_pos_clicked()
{
    int current_index = ui->tableWidget_robotarm_common_pos->currentRow();

    if(current_index == -1){
        QMessageBox::information(this,"提示\t","选中要更新行\t");
    }

    RobotPoseJoint data = mainthread_robot_arm_->getRobotPoseJoint();

    ui->tableWidget_robotarm_common_pos->setItem(current_index,1,new QTableWidgetItem(QString::number(data.joint1 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(current_index,2,new QTableWidgetItem(QString::number(data.joint2 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(current_index,3,new QTableWidgetItem(QString::number(data.joint3 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(current_index,4,new QTableWidgetItem(QString::number(data.joint4 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(current_index,5,new QTableWidgetItem(QString::number(data.joint5 , 'f', 6 )));
    ui->tableWidget_robotarm_common_pos->setItem(current_index,6,new QTableWidgetItem(QString::number(data.joint6 , 'f', 6 )));

    QMessageBox::information(this, "提示\t", "命名点位，最后选择保存\t");

}

void MainWindow::on_button_update_station_pos_clicked()
{
    int current_index = ui->tableWidget_robotarm_station_pos->currentRow();

    if(current_index == -1){
        QMessageBox::information(this,"提示\t","选中要更新行\t");
    }

    RobotPoseXYZ data = mainthread_robot_arm_->getRobotPoseXYZ();

    ui->tableWidget_robotarm_station_pos->setItem(current_index,1,new QTableWidgetItem(QString::number(data.x , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(current_index,2,new QTableWidgetItem(QString::number(data.y , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(current_index,3,new QTableWidgetItem(QString::number(data.z , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(current_index,4,new QTableWidgetItem(QString::number(data.rx , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(current_index,5,new QTableWidgetItem(QString::number(data.ry , 'f', 6 )));
    ui->tableWidget_robotarm_station_pos->setItem(current_index,6,new QTableWidgetItem(QString::number(data.rz , 'f', 6 )));

    QMessageBox::information(this, "提示\t", "命名点位，然后选择保存\t");
}

void MainWindow::on_pushButton_copy_station_clicked()
{
    QString station_id = QString::number(ui->spinBox_station_id->value());

    if(station_ids.find(station_id) != station_ids.end())
    {
        //QMessageBox::critical(this, "错误\t", "机台号已存在\t");
        //return;
    }
    else{
        station_ids.emplace(station_id);
        //保存数据到json文件
        write_stationid_tofile();
    }

    //拷贝数据到底层
    QString copy_station_id = ui->comboBox_station_name->currentText();
    QString target_station_id = QString::number(ui->spinBox_station_id->value());
    mainthread_robot_arm_->copy_station_data(copy_station_id, target_station_id);

    //上层界面更新
    if(station_ids.find(target_station_id) == station_ids.end()){
        ui->comboBox_station_name->addItem(target_station_id);
        ui->comboBox_station_name_matrix->addItem(target_station_id);
    }

    int target_index = target_station_id.toInt();

    Q_ASSERT(target_index >= 0);
    ui->comboBox_station_name->setCurrentIndex(target_index);

}

void MainWindow::on_pushButton_U_DO_1_clicked()
{
    int index = ui->comboBox_DO->currentIndex();
    switch(index)
    {
    case 0  : if(mainthread_robot_arm_->set_DO(0, 1)) ui->checkBox_U_DO_00->setCheckState(Qt::Checked); break;
    case 1  : if(mainthread_robot_arm_->set_DO(1, 1)) ui->checkBox_U_DO_01->setCheckState(Qt::Checked); break;
    case 2  : if(mainthread_robot_arm_->set_DO(2, 1)) ui->checkBox_U_DO_02->setCheckState(Qt::Checked); break;
    case 3  : if(mainthread_robot_arm_->set_DO(3, 1)) ui->checkBox_U_DO_03->setCheckState(Qt::Checked); break;
    case 4  : if(mainthread_robot_arm_->set_DO(4, 1)) ui->checkBox_U_DO_04->setCheckState(Qt::Checked); break;
    case 5  : if(mainthread_robot_arm_->set_DO(5, 1)) ui->checkBox_U_DO_05->setCheckState(Qt::Checked); break;
    case 6  : if(mainthread_robot_arm_->set_DO(6, 1)) ui->checkBox_U_DO_06->setCheckState(Qt::Checked); break;
    case 7  : if(mainthread_robot_arm_->set_DO(7, 1)) ui->checkBox_U_DO_07->setCheckState(Qt::Checked); break;
    case 8  : if(mainthread_robot_arm_->set_DO(8, 1)) ui->checkBox_U_DO_10->setCheckState(Qt::Checked); break;
    case 9  : if(mainthread_robot_arm_->set_DO(9, 1)) ui->checkBox_U_DO_11->setCheckState(Qt::Checked); break;
    case 10 : if(mainthread_robot_arm_->set_DO(10,1)) ui->checkBox_U_DO_12->setCheckState(Qt::Checked); break;
    case 11 : if(mainthread_robot_arm_->set_DO(11,1)) ui->checkBox_U_DO_13->setCheckState(Qt::Checked); break;
    case 12 : if(mainthread_robot_arm_->set_DO(12,1)) ui->checkBox_U_DO_14->setCheckState(Qt::Checked); break;
    case 13 : if(mainthread_robot_arm_->set_DO(13,1)) ui->checkBox_U_DO_15->setCheckState(Qt::Checked); break;
    case 14 : if(mainthread_robot_arm_->set_DO(14,1)) ui->checkBox_U_DO_16->setCheckState(Qt::Checked); break;
    case 15 : if(mainthread_robot_arm_->set_DO(15,1)) ui->checkBox_U_DO_17->setCheckState(Qt::Checked); break;
    default : break;
    }
}

void MainWindow::on_pushButton_U_DO_0_clicked()
{
    int index = ui->comboBox_DO->currentIndex();
    switch(index)
    {
    case 0  : if(mainthread_robot_arm_->set_DO(0, 0)) ui->checkBox_U_DO_00->setCheckState(Qt::Unchecked); break;
    case 1  : if(mainthread_robot_arm_->set_DO(1, 0)) ui->checkBox_U_DO_01->setCheckState(Qt::Unchecked); break;
    case 2  : if(mainthread_robot_arm_->set_DO(2, 0)) ui->checkBox_U_DO_02->setCheckState(Qt::Unchecked); break;
    case 3  : if(mainthread_robot_arm_->set_DO(3, 0)) ui->checkBox_U_DO_03->setCheckState(Qt::Unchecked); break;
    case 4  : if(mainthread_robot_arm_->set_DO(4, 0)) ui->checkBox_U_DO_04->setCheckState(Qt::Unchecked); break;
    case 5  : if(mainthread_robot_arm_->set_DO(5, 0)) ui->checkBox_U_DO_05->setCheckState(Qt::Unchecked); break;
    case 6  : if(mainthread_robot_arm_->set_DO(6, 0)) ui->checkBox_U_DO_06->setCheckState(Qt::Unchecked); break;
    case 7  : if(mainthread_robot_arm_->set_DO(7, 0)) ui->checkBox_U_DO_07->setCheckState(Qt::Unchecked); break;
    case 8  : if(mainthread_robot_arm_->set_DO(8, 0)) ui->checkBox_U_DO_10->setCheckState(Qt::Unchecked); break;
    case 9  : if(mainthread_robot_arm_->set_DO(9, 0)) ui->checkBox_U_DO_11->setCheckState(Qt::Unchecked); break;
    case 10 : if(mainthread_robot_arm_->set_DO(10,0)) ui->checkBox_U_DO_12->setCheckState(Qt::Unchecked); break;
    case 11 : if(mainthread_robot_arm_->set_DO(11,0)) ui->checkBox_U_DO_13->setCheckState(Qt::Unchecked); break;
    case 12 : if(mainthread_robot_arm_->set_DO(12,0)) ui->checkBox_U_DO_14->setCheckState(Qt::Unchecked); break;
    case 13 : if(mainthread_robot_arm_->set_DO(13,0)) ui->checkBox_U_DO_15->setCheckState(Qt::Unchecked); break;
    case 14 : if(mainthread_robot_arm_->set_DO(14,0)) ui->checkBox_U_DO_16->setCheckState(Qt::Unchecked); break;
    case 15 : if(mainthread_robot_arm_->set_DO(15,0)) ui->checkBox_U_DO_17->setCheckState(Qt::Unchecked); break;
    default : break;
    }
}

void MainWindow::on_pushButton_U_DI_getstatus_clicked()
{
    //
    /*
    if(mainthread_robot_arm_->get_DI(0))  ui->checkBox_U_DI_00->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(1))  ui->checkBox_U_DI_01->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(2))  ui->checkBox_U_DI_02->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(3))  ui->checkBox_U_DI_03->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(4))  ui->checkBox_U_DI_04->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(5))  ui->checkBox_U_DI_05->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(6))  ui->checkBox_U_DI_06->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(7))  ui->checkBox_U_DI_07->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(8))  ui->checkBox_U_DI_08->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(9))  ui->checkBox_U_DI_09->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(10)) ui->checkBox_U_DI_10->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(11)) ui->checkBox_U_DI_11->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(12)) ui->checkBox_U_DI_12->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(13)) ui->checkBox_U_DI_13->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(14)) ui->checkBox_U_DI_14->setCheckState(Qt::Checked);
    if(mainthread_robot_arm_->get_DI(15)) ui->checkBox_U_DI_15->setCheckState(Qt::Checked);


    if(! mainthread_robot_arm_->get_DI(0))  ui->checkBox_U_DI_00->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(1))  ui->checkBox_U_DI_01->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(2))  ui->checkBox_U_DI_02->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(3))  ui->checkBox_U_DI_03->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(4))  ui->checkBox_U_DI_04->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(5))  ui->checkBox_U_DI_05->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(6))  ui->checkBox_U_DI_06->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(7))  ui->checkBox_U_DI_07->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(8))  ui->checkBox_U_DI_08->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(9))  ui->checkBox_U_DI_09->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(10))  ui->checkBox_U_DI_10->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(11))  ui->checkBox_U_DI_11->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(12))  ui->checkBox_U_DI_12->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(13))  ui->checkBox_U_DI_13->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(14))  ui->checkBox_U_DI_14->setCheckState(Qt::Unchecked);
    if(! mainthread_robot_arm_->get_DI(15))  ui->checkBox_U_DI_15->setCheckState(Qt::Unchecked);
    */
}


void MainWindow::on_pushButton_openCamera_arm_clicked()
{
    bool state = camera_->turnOnCamera(CameraName::RobotArm);

    if(state == true){
        QMessageBox::information(this,"Open Camera ","Open Camera Success!");
        ui->checkBox_cameraState->setChecked(true);
    }
    else{
        QMessageBox::information(this,"Open Camera ","Open Camera Fail!");
        ui->checkBox_cameraState->setChecked(false);
    }
}

void MainWindow::on_pushButton_closeCamera_arm_clicked()
{

    timer_get_camera_image_->stop();

    bool state = camera_->turnOffCamera(CameraName::RobotArm);

    if(state == true){
        QMessageBox::information(this,"Close Camera ","Close Camera Success!");
        ui->checkBox_cameraState->setChecked(false);
    }
    else{
        QMessageBox::information(this,"Close Camera ","Close Camera Fail!");
        //ui->checkBox_cameraState->setChecked(true);
    }
}

void MainWindow::on_pushButton_load_picture_arm_clicked()
{
    timer_get_camera_image_->stop();

    if(!camera_->isOpen(CameraName::RobotArm)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    bool state = camera_->GetImage(CameraName::RobotArm, image);

    if(state == false || image.empty()){
        QMessageBox::information(this,"Open Camera ","Capture Image Fail!");
        return;
    }

    cv::imwrite("D:/image_arm_calibration.png",image);

    QR_image = image.clone();

    cv::Mat QTmat = image.clone();
    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

    //
    cv::circle(QTmat,  cv::Point2f(0,0),
               30, cv::Scalar(255,0,0), 5, -1); //第五个参数我设为-1，表明这是个实点。

    QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                           QTmat.cols, QTmat.rows, QTmat.step,
                           QImage::Format_RGB888);

    int width = ui->label_show->width();
    int height = ui->label_show->height();

    qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                     Qt::IgnoreAspectRatio,
                                                                     Qt::SmoothTransformation);

    //QPainter painter(ui->label_show);
    //显示图片
    //QString imagePath = "D:/Qt-Project/RobotArmEyeSystem/PhotoTest/drawTest.PNG";   //图片对应路径
    //QRect rect(0,0,qcolorshow.size().width(),qcolorshow.size().height());
    //painter.drawPixmap(rect,QPixmap::fromImage(qcolorshow));

    //ui->label_show->setPixmap(QPixmap::fromImage(qcolorshow));
    //ui->label_show->resize(qcolorshow.size());
    this->update();


}

void MainWindow::on_horizontalSlider_camera_1_exposureTime_valueChanged(int value)
{
    camera_->set_exposure_time(CameraName::RobotArm, value);
}

void MainWindow::on_horizontalSlider_camera_dm_exposureTime_valueChanged(int value)
{
    camera_->set_exposure_time(CameraName::DataMatrix, value);
}

void MainWindow::on_pushButton_openCamera_dm_clicked()
{
    bool state = camera_->turnOnCamera(CameraName::DataMatrix);

    if(state == true){
        QMessageBox::information(this,"Open Camera ","Open Camera Success!");
        ui->checkBox_cameraState_dm->setChecked(true);
    }
    else{
        QMessageBox::information(this,"Open Camera ","Open Camera Fail!");
        ui->checkBox_cameraState_dm->setChecked(false);
    }
}

void MainWindow::on_pushButton_closeCamera_dm_clicked()
{
    bool state = camera_->turnOnCamera(CameraName::DataMatrix);

    if(state == true){
        QMessageBox::information(this,"Open Camera ","Open Camera Success!");
        ui->checkBox_cameraState_dm->setChecked(false);
    }
    else{
        QMessageBox::information(this,"Open Camera ","Open Camera Fail!");
    }
}

void MainWindow::on_pushButton_capture_image_dm_clicked()
{
    if(!camera_->isOpen(CameraName::DataMatrix)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::DataMatrix, image);

    Dm_image = image.clone();
    cv::cvtColor(Dm_image,Dm_image,cv::COLOR_GRAY2RGB);

    //显示图像
    cv::Mat QTmat = image.clone();
    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);
    QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                           QTmat.cols, QTmat.rows, QTmat.step,
                           QImage::Format_RGB888);

    int width = ui->label_data_matrix->width();
    int height = ui->label_data_matrix->height();

    QImage qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows)
                              .scaled(width, height,
                                      Qt::IgnoreAspectRatio,
                                      Qt::SmoothTransformation);

    ui->label_data_matrix->setPixmap(QPixmap::fromImage(qcolorshow));
    ui->label_data_matrix->resize(qcolorshow.size());

}

/*Test 1*/

//Test1-Step1: 等待取生料请求信号
void MainWindow::on_pushButton_T1_S1_clicked()
{
    bool state = waitfor_taking_raw_signal(3000);

    if(state == true){
        set_test_state("Test1",1,true);
    }
    else{
        set_test_state("Test1",1,false);
    }


}

//Test1-Step2: 移动到取生料点前方
void MainWindow::on_pushButton_T1_S2_clicked()
{
    bool state = moveto_transfer_front_pose_from_initialpose();

    if(state == true){
        set_test_state("Test1",2,true);
    }
    else{
        set_test_state("Test1",2,false);
    }


}

//Test1-Step3: 等待转移平台允许信号
void MainWindow::on_pushButton_T1_S3_clicked()
{

      bool state = waitfor_transfer_allowing_signal(3000);

      if(state == true){
          set_test_state("Test1",3,true);
      }else{
          set_test_state("Test1",3,false);
      }
}

//Test1-Step4: 移动到取生料位
void MainWindow::on_pushButton_T1_S4_clicked()
{

  bool state = moveto_tranferplane_pose();

  if(state == true){
      set_test_state("Test1",4,true);
  }
  else{
      set_test_state("Test1",4,false);
  }
}

//Test1-Step5: 吸料
void MainWindow::on_pushButton_T1_S5_clicked()
{
    bool state = suck_workpiece();

    if(state == true){
        set_test_state("Test1",5,true);
    }
    else{
        set_test_state("Test1",5,false);
    }

}

//Test1-Step6: 移动到二次平台上方
void MainWindow::on_pushButton_T1_S6_clicked()
{

    bool state = moveto_locationplane_pose_above_from_transfer();

    if(state == true){
        //如果所有执行正确
        set_test_state("Test1",6,true);
    }
    else{
        set_test_state("Test1",6,false);
    }
}

//Test1-Step7: 吹料
void MainWindow::on_pushButton_T1_S7_clicked()
{
    bool state = chow_workpiece();

    if(state == true){
      set_test_state("Test1",7,true);
    }else{
      set_test_state("Test1",7,false);
    }
}

//Test1-Step8: 移动到吹DM码上方
void MainWindow::on_pushButton_T1_S8_clicked()
{
    bool state = moveto_chowing_DMcode_pose_above();

    if(state == true){
        set_test_state("Test1",8,true);
    }
    else{
        set_test_state("Test1",8,false);
    }

}

//Test1-Step9: 吹DM码
void MainWindow::on_pushButton_T1_S9_clicked()
{
    bool state = chow_DMcode();

    if(state == true){
       set_test_state("Test1",9,true);
    }
    else{
       set_test_state("Test1",9,false);
    }
}

//Test1-Step10: 从二次定位平台，回到待机点
void MainWindow::on_pushButton_T1_S10_clicked()
{
     bool state = moveto_initialpose_from_locationplane();

     if(state == true){
         set_test_state("Test1",10,true);
     }else{
         set_test_state("Test1",10,false);
     }

}

//Test1-Step11: 给转移平台允许信号
void MainWindow::on_pushButton_T1_S11_clicked()
{
    bool state = send_transfer_allowing_signal();

    if(state == true){
        set_test_state("Test1",11,true);
    }else{
        set_test_state("Test1",11,false);
    }
}


/* Test 2*/
//Test2-Step1: 等待取CNC料请求信号
void MainWindow::on_pushButton_T2_S1_clicked()
{
    bool state = waitfor_taking_workpiece_signal(30000);

    if(state == true){
         set_test_state("Test2",1,true);
    }else{
         set_test_state("Test2",1,false);
    }

}

//Test2-Step2: 移动到视觉拍照位
void MainWindow::on_pushButton_T2_S2_clicked()
{
     bool state = moveto_taking_image_pose();

     if(state == true){
          set_test_state("Test2",2,true);
     }
     else{
          set_test_state("Test2",2,false);
     }
}

//Test2-Step3: 视觉定位
void MainWindow::on_pushButton_T2_S3_clicked()
{
     bool state = process_image_and_correct_pose();

     if(state == true){
         set_test_state("Test2",3,true);
     }
     else{
         set_test_state("Test2",3,false);
     }
}

//Test2-Step4: 移动到CNC机台门口
void MainWindow::on_pushButton_T2_S4_clicked()
{

    bool state = moveto_CNC_front_door_from_visionpose();

    if(state == true){
           set_test_state("Test2",4,true);
    }
    else{
           set_test_state("Test2",4,false);
    }

}

//Test2-Step5: 等待开门信号
void MainWindow::on_pushButton_T2_S5_clicked()
{
    bool state = waitfor_opening_CNC_door(30000);

    if(state == true){
            set_test_state("Test2",5,true);
    }
    else{
            set_test_state("Test2",5,false);
    }


}

//Test2-Step6: 移动到CNC机台OK料位
void MainWindow::on_pushButton_T2_S6_clicked()
{

    //bool state = open_jaw();

    bool state = moveto_CNC_baseplane_taking_pose();

    //state = close_jaw();

    if(state == false){
        set_test_state("Test2",6,false);
    }

    set_test_state("Test2",6,true);
}

//Test2-Step7: 吸料
void MainWindow::on_pushButton_T2_S7_clicked()
{
    bool state = suck_workpiece();

    if(state == true){
          set_test_state("Test2",7,true);
    }else{
          set_test_state("Test2",7,false);
    }
}

//Test2-Step8: 移动到水槽
void MainWindow::on_pushButton_T2_S8_clicked()
{



    bool state = moveto_waterpool();

    if(state == false){
        set_test_state("Test2",8,false);
    }

    set_test_state("Test2",8,true);
}

//Test2-Step9: 上下移动并清洗
void MainWindow::on_pushButton_T2_S9_clicked()
{
    bool state = move_upanddown();

    if(state == true){
        set_test_state("Test2",9,true);
    }
    else{
        set_test_state("Test2",9,false);
    }

}

//Test2-Step10: 关闭抽水泵
void MainWindow::on_pushButton_T2_S10_clicked()
{
    bool state = close_washing_workpiece();

    if(state == false){
        set_test_state("Test2",10,false);
    }


   set_test_state("Test2",10,true);
}

//Test2-Step11: 移动到转移平台前方
void MainWindow::on_pushButton_T2_S11_clicked()
{
    //检测上一步状态
    bool state = moveto_transfer_front_pose_from_waterpool();

    if(state == false){
        set_test_state("Test2",11,false);
    }

    //
    set_test_state("Test2",11,true);
}

//Test2-Step12: 等待转移平台允许信号
void MainWindow::on_pushButton_T2_S12_clicked()
{
    bool state = waitfor_transfer_allowing_signal(30000);
    if(state == false){
        set_test_state("Test2",12,false);
    }


    set_test_state("Test2",12,true);
}

//Test2-Step13: 移动到转移平台放OK料位
void MainWindow::on_pushButton_T2_S13_clicked()
{
    bool state = moveto_tranferplane_unload_pose();

    if(state = false){
        set_test_state("Test2",13,false);
        return;
    }

    set_test_state("Test2",13,true);
}

//Test2-Step14: 吹料
void MainWindow::on_pushButton_T2_S14_clicked()
{
    bool state = chow_workpiece();

    if(state = false){
        set_test_state("Test2",14,false);
        return;
    }

    set_test_state("Test2",14,true);
}

//移动回待机点
void MainWindow::on_pushButton_T2_S15_clicked()
{
    bool state = moveto_initialpose_from_transfer();

    if(state == true){
          set_test_state("Test2",15,true);
    }
    else{
          set_test_state("Test2",15,false);
    }
}

// 给转移平台允许信号
void MainWindow::on_pushButton_T2_S16_clicked()
{
    bool state = send_transfer_allowing_signal();

    if(state == true){
          set_test_state("Test2",16,true);
    }
    else{
          set_test_state("Test2",16,false);
    }
}

//给agv取料完成信号
void MainWindow::on_pushButton_T2_S17_clicked()
{
    bool state = send_finishing_taking_signal();

    if(state == true){
          set_test_state("Test2",16,true);
    }
    else{
          set_test_state("Test2",16,false);
    }
}


/* Test-3 */
//Test3-Step1: 等待放料请求信号, 也是开门信号
void MainWindow::on_pushButton_T3_S1_clicked()
{
    bool state = waitfor_putting_raw_signal(3000000);

    if(state == true){
            set_test_state("Test3",1,true);
    }
    else{
            set_test_state("Test3",1,false);
    }
}

//Test3-Ste2: 读DM码
void MainWindow::on_pushButton_T3_S2_clicked(){


//    std::string message = "EDG0000012422086";

//    NDC_Client_.write_DM_code_to_NDC(message);
//    return;

    bool state = decode_DMcode();

    if(state == false){
        set_test_state("Test3",2,false);
    }

    set_test_state("Test3",2,true);

}


//Test3-Ste3: 移动到视觉拍照位
void MainWindow::on_pushButton_T3_S3_clicked()
{
    bool state = moveto_taking_image_pose();

    if(state == true){
        set_test_state("Test3",3,true);
    }
    else{
        set_test_state("Test3",3,false);
    }

}

//Test3-Step4: 视觉定位
void MainWindow::on_pushButton_T3_S4_clicked()
{

    bool state = process_image_and_correct_pose();

    if(state == false){
        set_test_state("Test3",4,false);
    }

    set_test_state("Test3",4,true);


}

//Test3-Step5: 移动到二次定位平台
void MainWindow::on_pushButton_T3_S5_clicked()
{
     bool state = moveto_locationplane_pose_from_visionpose();

     if(state == false){
         set_test_state("Test3",5,false);
     }

     set_test_state("Test3",5,true);
}

//Test3-Step6: 吸料
void MainWindow::on_pushButton_T3_S6_clicked()
{
    bool state = suck_workpiece();

    if(state == true){
        set_test_state("Test3",6,true);
    }
    else{
        set_test_state("Test3",6,false);
    }

}

//Test3-Step7: 移动到CNC机台门口
void MainWindow::on_pushButton_T3_S7_clicked()
{
    bool state = moveto_CNC_front_door_from_locationplane();

    if(state == false){
        set_test_state("Test3",7,false);
    }

    set_test_state("Test3",7,true);

}

//Test3-Step8: 等待开门信号
void MainWindow::on_pushButton_T3_S8_clicked()
{
    bool state = waitfor_opening_CNC_door(30000);


    if(state == true){
        set_test_state("Test3",8,true);
    }
    else{
        set_test_state("Test3",8,false);
    }


}

//Test3-Step9: 移动到CNC机台OK料位
void MainWindow::on_pushButton_T3_S9_clicked()
{
    bool state = moveto_CNC_baseplane_putting_pose();

    if(state == false){
        set_test_state("Test3",9,false);
    }

    set_test_state("Test3",9,true);
}

//Test3-Step10: 吹料
void MainWindow::on_pushButton_T3_S10_clicked()
{

    bool state = chow_workpiece();

    if(state == false){
        set_test_state("Test3",10,false);
    }

    //
    set_test_state("Test3",10,true);
}

//Test3-Step11: 移动回待机点
void MainWindow::on_pushButton_T3_S11_clicked()
{

    bool state = moveto_initialpose_from_CNC();

    if(state == false){
        set_test_state("Test3",11,false);
    }

    set_test_state("Test3",11,true);
}

//Test3-Step12: 给AGV放料完成信号
void MainWindow::on_pushButton_T3_S12_clicked()
{
    bool state = send_finishing_putting_signal();

    if(state = false){
        set_test_state("Test3",12,false);
        return;
    }

    set_test_state("Test3",12,true);
}

bool MainWindow::check_test_state(QString testName, int step_index){
    if(testName == "Test1"){
        if(test1_step_state[step_index] = 1){
            return true;
        }
        else{

            QMessageBox::information(this,"Test1", "Check Last step");
            return false;
        }
    }
}

void MainWindow::set_test_state(QString testName, int step_index, bool state){

    /*
    if(testName == "Test1"){

        if(state == true){
            test1_step_state[step_index] = 1;
        }else{
            test1_step_state[step_index] = 0;
        }


        switch (step_index) {
        case 1:
            if(state == true){
                ui->label_T1_S1->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S1->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 2:
            if(state == true){
                ui->label_T1_S2->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S2->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 3:
            if(state == true){
                ui->label_T1_S3->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S3->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;

        case 4:
            if(state == true){
                ui->label_T1_S4->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S4->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 5:
            if(state == true){
                ui->label_T1_S5->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S5->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 6:
            if(state == true){
                ui->label_T1_S6->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S6->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 7:
            if(state == true){
                ui->label_T1_S7->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S7->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 8:
            if(state == true){
                ui->label_T1_S8->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S8->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 9:
            if(state == true){
                ui->label_T1_S9->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S9->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        case 10:
            if(state == true){
                ui->label_T1_S10->setStyleSheet("background-color : rgb(0,255,0)");
                QMessageBox::information(this,"Test1", "Step Success");
            }
            else{
                ui->label_T1_S10->setStyleSheet("background-color : rgb(255,0,0)");
                QMessageBox::information(this,"Test1", "Step Fail");
            }
            break;
        default:
            break;
        }
    }
    */

    if(testName == "Test1"){

        if(state == true){
            test1_step_state[step_index] = 1;
            test1_qlabel[step_index]->setStyleSheet("background-color : rgb(0,255,0)");
            QMessageBox::information(this,"Test1", "Step Success");
        }else{
            test1_step_state[step_index] = 0;
            test1_qlabel[step_index]->setStyleSheet("background-color : rgb(255,0,0)");
            QMessageBox::information(this,"Test1", "Step Fail");
        }

    }




    if(testName == "Test2"){

        if(state == true){
            test2_step_state[step_index] = 1;
            test2_qlabel[step_index]->setStyleSheet("background-color : rgb(0,255,0)");
            QMessageBox::information(this,"Test2", "Step Success");
        }else{
            test2_step_state[step_index] = 0;
            test2_qlabel[step_index]->setStyleSheet("background-color : rgb(255,0,0)");
            QMessageBox::information(this,"Test2", "Step Fail");
        }

    }

    if(testName == "Test3"){

        if(state == true){
            test3_step_state[step_index] = 1;
            test3_qlabel[step_index]->setStyleSheet("background-color : rgb(0,255,0)");
            QMessageBox::information(this,"Test2", "Step Success");
        }else{
            test3_step_state[step_index] = 0;
            test3_qlabel[step_index]->setStyleSheet("background-color : rgb(255,0,0)");
            QMessageBox::information(this,"Test2", "Step Fail");
        }

    }

}

  void MainWindow::sleep_ms(int msecond){
      QTime dieTime = QTime::currentTime().addMSecs(msecond);
      while( QTime::currentTime() < dieTime ){
          QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
      }
  }

  void MainWindow::set_test_qlabel(){

      test1_qlabel[1] = ui->label_T1_S1;
      test1_qlabel[2] = ui->label_T1_S2;
      test1_qlabel[3] = ui->label_T1_S3;
      test1_qlabel[4] = ui->label_T1_S4;
      test1_qlabel[5] = ui->label_T1_S5;
      test1_qlabel[6] = ui->label_T1_S6;
      test1_qlabel[7] = ui->label_T1_S7;
      test1_qlabel[8] = ui->label_T1_S8;
      test1_qlabel[9] = ui->label_T1_S9;
      test1_qlabel[10] = ui->label_T1_S10;
      test1_qlabel[11] = ui->label_T1_S11;

      test2_qlabel[1] = ui->label_T2_S1;
      test2_qlabel[2] = ui->label_T2_S2;
      test2_qlabel[3] = ui->label_T2_S3;
      test2_qlabel[4] = ui->label_T2_S4;
      test2_qlabel[5] = ui->label_T2_S5;
      test2_qlabel[6] = ui->label_T2_S6;
      test2_qlabel[7] = ui->label_T2_S7;
      test2_qlabel[8] = ui->label_T2_S8;
      test2_qlabel[9] = ui->label_T2_S9;
      test2_qlabel[10] = ui->label_T2_S10;
      test2_qlabel[11] = ui->label_T2_S11;
      test2_qlabel[12] = ui->label_T2_S12;
      test2_qlabel[13] = ui->label_T2_S13;
      test2_qlabel[14] = ui->label_T2_S14;
      test2_qlabel[15] = ui->label_T2_S15;
      test2_qlabel[16] = ui->label_T2_S16;
      test2_qlabel[17] = ui->label_T2_S17;

      test3_qlabel[1] = ui->label_T3_S1;
      test3_qlabel[2] = ui->label_T3_S2;
      test3_qlabel[3] = ui->label_T3_S3;
      test3_qlabel[4] = ui->label_T3_S4;
      test3_qlabel[5] = ui->label_T3_S5;
      test3_qlabel[6] = ui->label_T3_S6;
      test3_qlabel[7] = ui->label_T3_S7;
      test3_qlabel[8] = ui->label_T3_S8;
      test3_qlabel[9] = ui->label_T3_S9;
      test3_qlabel[10] = ui->label_T3_S10;
      test3_qlabel[11] = ui->label_T3_S11;
      test3_qlabel[12] = ui->label_T3_S12;

  }

void MainWindow::on_pushButton_add_rotation_pose_clicked()
{

    timer_get_camera_image_->stop();

    if(!camera_->isOpen(CameraName::RobotArm)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::RobotArm, image);
    QR_image = image.clone();

    //cv::Mat QTmat = image.clone();
    //cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

    //
    //cv::circle(QTmat,  cv::Point2f(0,0),
    //           30, cv::Scalar(255,0,0), 5, -1); //第五个参数我设为-1，表明这是个实点。

    //QImage qcolor = QImage((const unsigned char*)(QTmat.data),
    //                       QTmat.cols, QTmat.rows, QTmat.step,
    //                       QImage::Format_RGB888);

    //int width = ui->label_show->width();
    //int height = ui->label_show->height();

    //qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
    //                                                                 Qt::IgnoreAspectRatio,
    //                                                                 Qt::SmoothTransformation);

    //this->update();

    if(QR_image.empty()) {
        QMessageBox::information(this, "提示\t", "请加载图像或使用相机采集图像\t");
    }

    //input
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();
    QString readDir = DirPath + DirName + station_id;

    //output
    std::vector<cv::Point2d> corners;
    cv::Point2d center;
    float angle;

    bool match_state = image_processor_.match_image_model(readDir , station_id,
                                       QR_image,
                                       corners,
                                       center,
                                       angle);

    //qDebug() << "center " << center.x << " , " <<center.y;

    if(match_state == true){


        if(center.x == 0 && center.y == 0){
            QMessageBox::information(this, "提示\t", "匹配失败\t");
            return;
        }

        fitting_circle_points.push_back(center);

        cv::Mat QTmat = QR_image.clone();
        cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);


        for(int j=0; j< fitting_circle_points.size(); j++)
        {
            cv::Point p2;
            p2.x = fitting_circle_points[j].x;
            p2.y = fitting_circle_points[j].y;
            int size = image.size().height * 0.05;

            //cv::circle(Image, p2, 3, cv::Scalar(0,0,255),5,-1); //第五个参数我设为-1，表明这是个实点。
            cv::line(QTmat,cv::Point(p2.x-size/2,p2.y),cv::Point(p2.x+size/2,p2.y),cv::Scalar(255,0,0),2,8,0);
            cv::line(QTmat,cv::Point(p2.x,p2.y-size/2),cv::Point(p2.x,p2.y+size/2),cv::Scalar(255,0,0),2,8,0);

        }

        QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                               QTmat.cols, QTmat.rows, QTmat.step,
                               QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                         Qt::IgnoreAspectRatio,
                                                                         Qt::SmoothTransformation);
        this->update();

    }

}

void MainWindow::on_pushButton_find_center_clicked()
{

        //int center_x = QR_image.cols/2;
        //int center_y = QR_image.rows/2;

        //qDebug() << "center_x " << center_x;
        //qDebug() << "center_y " << center_y;

        //
        //cv::Point2d p1(1298.89  ,  1569.4);
        //cv::Point2d p2(1234.02  ,  1219.78);
        //cv::Point2d p3(1271.23  ,  770.537);
        //cv::Point2d p4(1384.68  ,  453.478);

        //fitting_circle_points.push_back(p1);
        //fitting_circle_points.push_back(p2);
        //fitting_circle_points.push_back(p3);
        //fitting_circle_points.push_back(p4);

    //center  1384.85  ,  454.026
    //center  1248.7  ,  889.36
    //center  1250.04  ,  1359.12

        for(auto it : fitting_circle_points)
        {
            //qDebug() << "( " << it.x << " " << it.y << ")";
        }

        //计算旋转中心
        if (fitting_circle_points.size() >= 3)
        {
            double radius;
            cv::Point2d mark_center;
            calibration_.circleLeastFit(fitting_circle_points,mark_center,radius);

            //
            cv::Point p2;
            p2.x = mark_center.x;
            p2.y = mark_center.y;

            //qDebug() << "mark_center ("
            //         << mark_center.x << " " << mark_center.y << ")"
            //         << " radius " << radius;

            cv::Mat QTmat = QR_image.clone();
            cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

            cv::circle(QTmat, p2, 3, cv::Scalar(0,0,255),10,-1); //第五个参数我设为-1，表明这是个实点。
            cv::circle(QTmat, p2, radius, cv::Scalar(0,0,255),3,cv::LINE_8); //第五个参数我设为-1，表明这是个实点。

            for(int j=0; j< fitting_circle_points.size(); j++)
            {
                cv::Point p2;
                p2.x = fitting_circle_points[j].x;
                p2.y = fitting_circle_points[j].y;
                int size = QTmat.size().height * 0.05;

                //cv::circle(Image, p2, 3, cv::Scalar(0,0,255),5,-1); //第五个参数我设为-1，表明这是个实点。
                cv::line(QTmat,cv::Point(p2.x-size/2,p2.y),cv::Point(p2.x+size/2,p2.y),cv::Scalar(255,0,0),2,8,0);
                cv::line(QTmat,cv::Point(p2.x,p2.y-size/2),cv::Point(p2.x,p2.y+size/2),cv::Scalar(255,0,0),2,8,0);

            }

            QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                                   QTmat.cols, QTmat.rows, QTmat.step,
                                   QImage::Format_RGB888);

            int width = ui->label_show->width();
            int height = ui->label_show->height();

            qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                             Qt::IgnoreAspectRatio,
                                                                             Qt::SmoothTransformation);
            this->update();
            //robot_arm_.robot_sleep(3000);
            QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("find center success!"));

        }
        else
        {
            QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("at least 3 point!"));
            //return false;
        }


        //move back to origin point
        RobotPoseXYZ pose = {
            ui->tableWidget_reference_pose->item(0,0)->text().toDouble(),
            ui->tableWidget_reference_pose->item(0,1)->text().toDouble(),
            ui->tableWidget_reference_pose->item(0,2)->text().toDouble(),
            ui->tableWidget_reference_pose->item(0,3)->text().toDouble(),
            ui->tableWidget_reference_pose->item(0,4)->text().toDouble(),
            ui->tableWidget_reference_pose->item(0,5)->text().toDouble()
        };


        mainthread_robot_arm_->move_to_pose(pose,
                                            MoveType::JOINT,
                                            JointSpeed,JointACC,JointDEC,
                                            BlockMotion::LOCKED);

        //清空数据
        fitting_circle_points.clear();

        QMessageBox::information(this, QStringLiteral("提示"), QStringLiteral("move back to origin!"));
        //return false;


}

void MainWindow::upPaint(){
    QPainter painter(ui->label_show);
    //显示图片
    //QString imagePath = "D:/Qt-Project/RobotArmEyeSystem/PhotoTest/drawTest.PNG";   //图片对应路径
    //painter.drawPixmap(0,0,400,300,QPixmap(imagePath));

    //显示图片的另一种方法：setPixmap,这里不要使用
    //QImage Image;
    //Image.load(imagePath);
    //ui->lb_image->setPixmap(QPixmap::fromImage(Image));

    if(!qcolorshow.isNull()){

        QRect rect(0,0,qcolorshow.size().width(),qcolorshow.size().height());

        QPixmap pixmap = QPixmap::fromImage(qcolorshow);
        //painter.drawPixmap(rect, pixmap);

        //
        //QPainter painter(this);
        double width=pixmap.width()*m_scaleValue;
        double height=pixmap.height()*m_scaleValue;
        QPixmap scalePixmap=pixmap.scaled(width,height,Qt::IgnoreAspectRatio, Qt::SmoothTransformation); // 饱满缩放
        m_rectPixmap = QRect(m_drawPoint.x(),m_drawPoint.y(),width,height);  // 图片区域
        painter.drawPixmap(m_rectPixmap,scalePixmap);

        //qDebug() << "m_rectPixmap " << m_rectPixmap;
    }

    QPen pen;
    pen.setColor(Qt::red);
    painter.setPen(pen);
    //painter.drawRect(1,1,100,50);
    //painter.drawText(10,30,"测试文字");

    if(rectFlag)   //line2
    {
        painter.drawRect(rect[0],rect[1],rect[2],rect[3]);
    }

}

void MainWindow::mousePressEvent(QMouseEvent *event){
    if(event->button() == Qt::LeftButton)
    {
        isPressed = true;
        if(rectFlag)
        {
            rect[0] = event->pos().x() - offset;
            rect[1] = event->pos().y() - offset;

        //qDebug()<<"press:"<<QPoint(event->pos().x(),event->pos().y());
        }
    }

    //other code
    if(event->button()==Qt::LeftButton && !rectFlag)
    {
            m_isMousePress=true;
            m_mousePoint=event->pos();
    }
}

void MainWindow::mouseMoveEvent(QMouseEvent *event){
    if(isPressed)
    {
        if(rectFlag)
        {
            rect[2] = event->pos().x() - rect[0] - offset;
            rect[3] = event->pos().y()- rect[1] - offset;

           //qDebug()<<"move:"<<QPoint(event->pos().x(),event->pos().y());
        }
        this->update();
    }

    //other code
    if(m_isMousePress && !rectFlag)
    {
        int x=event->pos().x()-m_mousePoint.x();
        int y=event->pos().y()-m_mousePoint.y();
        m_mousePoint=event->pos();
        m_drawPoint=QPointF(m_drawPoint.x()+x,m_drawPoint.y()+y);
        update();
    }
}

void MainWindow::mouseReleaseEvent(QMouseEvent *event){
    //qDebug()<<"Release:"<<QPoint(event->pos().x(),event->pos().y());
    if(event->button() == Qt::LeftButton)
    {
        if(rectFlag)
        {
            isPressed=false;
            rect[2] = event->pos().x() - rect[0] - offset;
            rect[3] = event->pos().y() - rect[1] - offset ;
            this->update();
        }
    }
    else if(event->button() == Qt::RightButton)
    {
        isPressed=false;
        rectFlag = false;
        this->update();
    }

    //other code
    if(event->button() == Qt::RightButton)
    {
        m_drawPoint=QPointF(0,0);
        m_scaleValue=1.0;
        rectFlag = false;
        update();
    }

    if(event->button() == Qt::LeftButton) {
        m_isMousePress=false;
    }
}

void MainWindow::wheelEvent(QWheelEvent *event) {
    changeWheelValue(event->pos(),event->delta());
    event->accept();
}

void MainWindow::resizeEvent(QResizeEvent *event) {
    m_drawPoint=QPointF(0,0);
    m_scaleValue=1.0;
    update();
}

void MainWindow::changeWheelValue(QPoint event,int numSteps){
    double oldScale = m_scaleValue;
    if(numSteps>0)
    {
        m_scaleValue*=1.1;
    }
    else
    {
        m_scaleValue*=0.9;
    }
    if(m_scaleValue>(SCALE_MAX_VALUE))
    {
        m_scaleValue=SCALE_MAX_VALUE;
    }
    if(m_scaleValue<(SCALE_MIN_VALUE))
    {
        m_scaleValue=SCALE_MIN_VALUE;
    }

    if(m_rectPixmap.contains(event))
    {
        double x=m_drawPoint.x()-(event.x()- offset - m_drawPoint.x())/m_rectPixmap.width()*(this->width()*(m_scaleValue-oldScale));
        double y=m_drawPoint.y()-(event.y()- offset - m_drawPoint.y())/m_rectPixmap.height()*(this->height()*(m_scaleValue-oldScale));
        m_drawPoint=QPointF(x,y);
    }
    else
    {
        double x=m_drawPoint.x()-(this->width()*(m_scaleValue-oldScale))/2;
        double y=m_drawPoint.y()-(this->height()*(m_scaleValue-oldScale))/2;
        m_drawPoint=QPointF(x,y);
    }
    this->update();
}

bool MainWindow::eventFilter(QObject *watched, QEvent *event){
    if(watched ==ui->label_show && event->type() == QEvent::Paint)
    {
        upPaint();
    }
    return QWidget::eventFilter(watched,event);
}

void MainWindow::on_pushButton_draw_model_clicked()
{
    rectFlag = true;
    m_isMousePress = false;
    this->update();
}

void MainWindow::on_pushButton_create_model_clicked()
{
    // show model image;

    if(rect[2] == 0 || rect[3] ==  0){

        QMessageBox::information(this,"CreateModel", "Draw model region firstly");
        return;
    }

//    qDebug() << "m_rectPixmap" << m_rectPixmap;
//    qDebug() << "start point (" << rect[0] << " , " << rect[1] << "}"
//             << " width: " << rect[2]
//             << " height: " << rect[3];

    //ROI

    float pic_scale_cols = 1.0*QR_image.cols / m_rectPixmap.width(); //ui->label_show->geometry().width();
    float pic_scale_row = 1.0*QR_image.rows / m_rectPixmap.height(); //ui->label_show->geometry().height();

//    qDebug() << "pic_scale_cols " << pic_scale_cols;
//    qDebug() << "pic_scale_row " << pic_scale_row;


    cv::Rect scale_rect(( rect[0] - m_rectPixmap.x()) * pic_scale_cols,
            ( rect[1] - m_rectPixmap.y()) * pic_scale_row,
            rect[2] * pic_scale_cols,
            rect[3] * pic_scale_row);

    cv::Mat qr_image_roi = QR_image(scale_rect);

    //cv::imshow("qr_image_roi",qr_image_roi);
    //cv::imwrite("D:/qr_image_roi.png",qr_image_roi);
    //cv::waitKey(10);

    //create image model

    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();

    QString saveDir = DirPath + DirName + station_id;
    //save template data to json
    QDir dir(saveDir);
    if (!dir.exists()) dir.mkdir(saveDir);

    bool createinfo = image_processor_.create_model(station_id, qr_image_roi, saveDir);

    if(createinfo == true){

        QFile StationModelData(DirPath +
                               DirName +
                               station_id + "\\" +
                               station_id + ".json");

        //| QIODevice::Append
        if(!StationModelData.open(QIODevice::WriteOnly | QIODevice::Text ))
        {
            return;
        }

        QJsonArray array;
        array.append(qr_image_roi.cols);
        array.append(qr_image_roi.rows);

        QJsonObject jsonObj;
        jsonObj.insert("info",array);

        QJsonDocument jsonDoc;
        jsonDoc.setObject(jsonObj);
        StationModelData.write(jsonDoc.toJson());
        StationModelData.close();
        QMessageBox::information(this, "提示\t", "数据模板创建完成\t");
    }

}

void MainWindow::on_pushButton_match_model_clicked()
{

    if(QR_image.empty()) {
        QMessageBox::information(this, "提示\t", "请加载图像或使用相机采集图像\t");
    }

    //input
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();
    QString readDir = DirPath + DirName + station_id;

    //output
    std::vector<cv::Point2d> corners;
    cv::Point2d center;
    float angle;

    bool match_state = image_processor_.match_image_model(readDir , station_id,
                                       QR_image,
                                       corners,
                                       center,
                                       angle);

    if(center.x != 0 && center.y != 0){

        cv::Mat QTmat = QR_image.clone();

        if(QTmat.type() == CV_8U){
            cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);
        }

        cv::circle(QTmat, {int(center.x), int(center.y)}, 10, cv::Scalar(255,0,0), -1);

//        cv::RotatedRect rotatedRectangle({center, center},
//                                         {2*r_scaled_width,  2*r_scaled_height},
//                                         -infos[match.template_id].angle);

        //cv::Point2f vertices[4];
        //rotatedRectangle.points(vertices);
        if(corners.size() == 4){
            for(int i=0; i<4; i++){
                int next = (i+1==4) ? 0 : (i+1);
                cv::line(QTmat, corners[i], corners[next], cv::Scalar(255,0,0), 2);
            }
        }

        //cv::imwrite("D:/show.png",QTmat);


        QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                               QTmat.cols, QTmat.rows, QTmat.step,
                               QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                         Qt::IgnoreAspectRatio,
                                                                         Qt::SmoothTransformation);

        this->update();

    }
    else{
        QMessageBox::information(this, "提示\t", "匹配失败\t");
    }


}

void MainWindow::on_button_load_station_template_info_clicked()
{
    //读取所有机台信号
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "StationsModel/";

        QString RobotPoseData_Dir = DirPath +
                                    DirName +
                                    it + "//" +
                                    it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无模板数据\t";
            QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                QJsonValue jvalue = obj[key];
                int image_info[2];
                if(jvalue.isArray()){
                    QJsonArray array = jvalue.toArray();
                    int nSize = array.size();

                    for(int i=0; i < nSize; ++i){
                        QJsonValue value = array.at(i);
                        if(value.isDouble()){
                            image_info[i] = value.toDouble();
                        }
                    }
                }

                if(image_info[0] != 0 && image_info[1] != 0){
                    ModelInfo model_info = {image_info[0], image_info[1]};
                    image_processor_.add_station_model_info(it,model_info);
                }

            }
        }

    }

    //显示所有机台数据
    /*
    auto station_model_info = image_processor_.get_station_model_info();

    for(auto it : station_model_info){

        //write to table
        int countIndex = ui->tableWidget_stations_model_info->rowCount();

        ui->tableWidget_stations_model_info->setRowCount(countIndex + 1);
        ui->tableWidget_stations_model_info->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

        ui->tableWidget_stations_model_info->setItem(countIndex,0,new
                                              QTableWidgetItem(it.first));
        ui->tableWidget_stations_model_info->setItem(countIndex,1,
                                              new QTableWidgetItem(QString::number(it.second.width)));
        ui->tableWidget_stations_model_info->setItem(countIndex,2,
                                              new QTableWidgetItem(QString::number(it.second.height)));

    }
    */

    QMessageBox::information(this, "提示\t", "加载机台标定数据完成\t");
}

void MainWindow::on_pushButton_rotate_fixed_angle_pressed()
{
    //获得当前机械臂坐标
    RobotPoseJoint robot_pose = mainthread_robot_arm_->getRobotPoseJoint();

    //检测机械臂坐标获取是否准确
    bool isGet = true;
    if( robot_pose.joint1 == 0
        && robot_pose.joint2 == 0
        && robot_pose.joint3 == 0){
        isGet = false;
    }

    //拷贝一份示教原点的数据
    float position_origin[6] = { robot_pose.joint1, robot_pose.joint2, robot_pose.joint3,
                                 robot_pose.joint4, robot_pose.joint5, robot_pose.joint6 };

    float position_temp[6] = { robot_pose.joint1, robot_pose.joint2, robot_pose.joint3,
                               robot_pose.joint4, robot_pose.joint5, robot_pose.joint6 };

    //如果获得当前点位
    RobotPoseJoint pose_tomove;
    if (true == isGet)
    {

        double value = ui->doubleSpinBox_rotateInterval->value();
        //旋转最后一个轴
        position_temp[5] = position_temp[5] + value;
        //移动到计算旋转中心的起点
        pose_tomove = {position_temp[0], position_temp[1], position_temp[2],
                       position_temp[3], position_temp[4], position_temp[5]};
        mainthread_robot_arm_->move_to_pose(pose_tomove,
                                           MoveType::JOINT,
                                           JointSpeed,JointACC,JointDEC,
                                           BlockMotion::UNLOCKED);
        //等待2秒使机械手稳定
        //mainthread_robot_arm_->robot_sleep(2000);
    }
}

void MainWindow::on_pushButton_rotate_fixed_angle_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_set_reference_pose_clicked()
{
    ui->tableWidget_reference_pose->setRowCount(0);
    RobotPoseXYZ data = mainthread_robot_arm_->getRobotPoseXYZ();

    int countIndex = ui->tableWidget_reference_pose->rowCount();

    ui->tableWidget_reference_pose->setRowCount(countIndex + 1);
    ui->tableWidget_reference_pose->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

    ui->tableWidget_reference_pose->setItem(countIndex,0,new QTableWidgetItem(QString::number(data.x , 'f', 6 )));
    ui->tableWidget_reference_pose->setItem(countIndex,1,new QTableWidgetItem(QString::number(data.y , 'f', 6 )));
    ui->tableWidget_reference_pose->setItem(countIndex,2,new QTableWidgetItem(QString::number(data.z , 'f', 6 )));
    ui->tableWidget_reference_pose->setItem(countIndex,3,new QTableWidgetItem(QString::number(data.rx , 'f', 6 )));
    ui->tableWidget_reference_pose->setItem(countIndex,4,new QTableWidgetItem(QString::number(data.ry , 'f', 6 )));
    ui->tableWidget_reference_pose->setItem(countIndex,5,new QTableWidgetItem(QString::number(data.rz , 'f', 6 )));

}

void MainWindow::on_pushButton_moveto_reference_pose_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    QString station_id = ui->comboBox_station_name_matrix->currentText();
    int row_count = ui->tableWidget_reference_pose->rowCount();

    if(row_count < 1){
        QMessageBox::critical(this, "错误\t", "请先设置参考位姿\t");
        return;
    }


    //QString pose_name = ui->tableWidget_robotarm_station_pos->item(current_index,0)->text();

    RobotPoseXYZ pose = {
        ui->tableWidget_reference_pose->item(0,0)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,1)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,2)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,3)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,4)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,5)->text().toDouble()
    };


    mainthread_robot_arm_->move_to_pose(pose,
                                        MoveType::JOINT,
                                        JointSpeed,JointACC,JointDEC,
                                        BlockMotion::UNLOCKED);

}

void MainWindow::on_pushButton_moveto_reference_pose_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_match_reference_pose_clicked()
{
    timer_get_camera_image_->stop();

    light_->setLuminance2(255);

    if(!camera_->isOpen(CameraName::RobotArm)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::RobotArm, image);
    QR_image = image.clone();

    if(QR_image.empty()) {
        QMessageBox::information(this, "提示\t", "请加载图像或使用相机采集图像\t");
    }

    //input
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();
    QString readDir = DirPath + DirName + station_id;

    //output
    std::vector<cv::Point2d> corners;
    cv::Point2d center;
    float angle;

    bool match_state = image_processor_.match_image_model(readDir , station_id,
                                       QR_image,
                                       corners,
                                       center,
                                       angle);
    //qDebug() << "angle " << angle;

    if(center.x != 0 && center.y != 0){

        ui->doubleSpinBox_reference_value->setValue(angle);


        calibration_.add_station_reference_angle(station_id,angle);

        cv::Mat QTmat = QR_image.clone();
        cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

        cv::circle(QTmat, {int(center.x), int(center.y)}, 10, cv::Scalar(255,0,0), -1);

        for(int i=0; i<4; i++){
            int next = (i+1==4) ? 0 : (i+1);
            cv::line(QTmat, corners[i], corners[next], cv::Scalar(255,0,0), 2);
        }

        QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                               QTmat.cols, QTmat.rows, QTmat.step,
                               QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                         Qt::IgnoreAspectRatio,
                                                                         Qt::SmoothTransformation);

        this->update();

    }

    light_->setLuminance2(0);


}

void MainWindow::on_pushButton_openCamera_arm_continue_clicked()
{
    timer_get_camera_image_->start(500);
}

void MainWindow::on_tableWidget_autoCalibration_activated(const QModelIndex &index)
{

}

bool MainWindow::get_image_and_match(QString station_id, cv::Point2d& center, float& angle){
      //停止连续采集
      timer_get_camera_image_->stop();

      if(!camera_->isOpen(CameraName::RobotArm)){
          QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
          set_test_state("Test2",2,false);
          return false;
      }

      cv::Mat image;
      bool state = camera_->GetImage(CameraName::RobotArm, image);

      //cv::imwrite("D:\image\image.png",image);
      if(state == false || image.empty()){
          qDebug() << "capture image fail";
          return false;
      }

      //cv::imwrite("D:/image_arm.png",image);

      QR_image = image.clone();

      //加载模板进行匹配
      //input
      QString DirPath = QCoreApplication::applicationDirPath() + "\\";
      QString DirName = "StationsModel\\";
      //QString station_id = ui->comboBox_station_name_matrix->currentText();
      QString readDir = DirPath + DirName + station_id;

      //output
      std::vector<cv::Point2d> corners;
      cv::Point2d qr_center;
      float qr_angle;

      bool match_state = image_processor_.match_image_model(readDir , station_id,
                                         QR_image,
                                         corners,
                                         qr_center,
                                         qr_angle);
//      qDebug() << "current_station_id " << current_station_id << " -------------------";
//      qDebug() << "angle " << angle;
//      qDebug() << "qr_center.x " << qr_center.x;
//      qDebug() << "qr_center.y " << qr_center.y;

      if(qr_center.x > 0 && qr_center.x < QR_image.cols &&
         qr_center.y > 0 && qr_center.y < QR_image.rows){

          center = qr_center;
          angle = qr_angle;

          cv::Mat QTmat = QR_image.clone();
          cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

          cv::circle(QTmat, {int(center.x), int(center.y)}, 10, cv::Scalar(255,0,0), -1);

          for(int i=0; i<4; i++){
              int next = (i+1==4) ? 0 : (i+1);
              cv::line(QTmat, corners[i], corners[next], cv::Scalar(255,0,0), 2);
          }

          QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                                 QTmat.cols, QTmat.rows, QTmat.step,
                                 QImage::Format_RGB888);

          int width = ui->label_show->width();
          int height = ui->label_show->height();

          qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                           Qt::IgnoreAspectRatio,
                                                                           Qt::SmoothTransformation);

          this->update();

          QDateTime current = QDateTime::currentDateTimeUtc();
          std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
          std::string id = current_station_id.toStdString();
          std::string save_dir = "D:/QR_code/OK/" + id + "__" + datetime + ".png";
          //qDebug() << "save_dir " << QString::fromStdString(save_dir);
          //cv::imwrite(save_dir,image);

          return true;

      }
      else{


          QDateTime current = QDateTime::currentDateTimeUtc();
          std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
          std::string id = current_station_id.toStdString();
          std::string save_dir = "D:/QR_code/NG/" + id + "__" + datetime + ".png";
          //qDebug() << "save_dir " << QString::fromStdString(save_dir);
          cv::imwrite(save_dir,image);

          return false;
      }

      /*
      RobotPoseXYZ reference_pose;
      bool isExist =  mainthread_robot_arm_->getRobotStationPose(station_id,
                                                                     "reference",
                                                                     reference_pose);
      if(isExist == false){
          return false;
      }

      //RobotPoseXYZ correct_pose;
      bool state = calibration_.correct_station_pose(station_id,
                                        center,
                                        angle,
                                        reference_pose,
                                        reference_pose,
                                        station_correct_pose);

      if(state == false) {
          return false;
      }*/


}

void MainWindow::initial_station_mapper()
{
    station_id_mapper.emplace("116","2208377");
    station_id_mapper.emplace("117","2208330");
    station_id_mapper.emplace("118","2208376");
    station_id_mapper.emplace("119","2208391");
    station_id_mapper.emplace("120","2208296");
    station_id_mapper.emplace("121","2208368");
    station_id_mapper.emplace("122","2208370");
    station_id_mapper.emplace("123","2208301");
    station_id_mapper.emplace("124","2208291");
    station_id_mapper.emplace("125","2208392");
    station_id_mapper.emplace("126","2208382");
    station_id_mapper.emplace("127","2208321");
    station_id_mapper.emplace("128","2208364");
    station_id_mapper.emplace("129","2209023");
    station_id_mapper.emplace("130","2209045");
    station_id_mapper.emplace("131","8203183");
    station_id_mapper.emplace("132","2209022");
    station_id_mapper.emplace("133","2208948");
    station_id_mapper.emplace("134","8203141");
    station_id_mapper.emplace("135","8203184");
    station_id_mapper.emplace("136","2208835");
    station_id_mapper.emplace("137","2208823");
    station_id_mapper.emplace("138","2208850");
    station_id_mapper.emplace("139","2208322");
    station_id_mapper.emplace("140","2208305");
    station_id_mapper.emplace("141","82100371");

}

void MainWindow::on_pushButton_correct_base_pose_clicked()
{
    timer_get_camera_image_->stop();

    light_->setLuminance2(255);

    if(!camera_->isOpen(CameraName::RobotArm)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::RobotArm, image);
    QR_image = image.clone();

    if(QR_image.empty()) {
        QMessageBox::information(this, "提示\t", "请加载图像或使用相机采集图像\t");
        return;
    }

    //input
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "StationsModel\\";
    QString station_id = ui->comboBox_station_name_matrix->currentText();
    QString readDir = DirPath + DirName + station_id;

    //output
    std::vector<cv::Point2d> corners;
    cv::Point2d center;
    float angle;

    bool match_state = image_processor_.match_image_model(readDir , station_id,
                                       QR_image,
                                       corners,
                                       center,
                                       angle);


    if(match_state == true){

        cv::Mat QTmat = QR_image.clone();
        cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);

        cv::circle(QTmat, {int(center.x), int(center.y)}, 10, cv::Scalar(255,0,0), -1);

        //rotatedRectangle.points(vertices);
        for(int i=0; i<4; i++){
            int next = (i+1==4) ? 0 : (i+1);
            cv::line(QTmat, corners[i], corners[next], cv::Scalar(255,0,0), 2);
        }

        QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                               QTmat.cols, QTmat.rows, QTmat.step,
                               QImage::Format_RGB888);

        int width = ui->label_show->width();
        int height = ui->label_show->height();

        qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows).scaled(width, height,
                                                                         Qt::IgnoreAspectRatio,
                                                                         Qt::SmoothTransformation);

        this->update();

    }
    else{

        return;
    }


    RobotPoseXYZ reference_pose = {
        ui->tableWidget_reference_pose->item(0,0)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,1)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,2)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,3)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,4)->text().toDouble(),
        ui->tableWidget_reference_pose->item(0,5)->text().toDouble()
    };


    RobotPoseXYZ correct_pose;

    calibration_.get_base_pose(station_id,
                               center,
                               angle,
                               reference_pose,
                               correct_pose);


    ui->tableWidget_base_pose->setRowCount(0);
    int countIndex = ui->tableWidget_base_pose->rowCount();
    ui->tableWidget_base_pose->setRowCount(countIndex + 1);
    ui->tableWidget_base_pose->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

    ui->tableWidget_base_pose->setItem(countIndex,0,new QTableWidgetItem(QString::number(correct_pose.x , 'f', 6 )));
    ui->tableWidget_base_pose->setItem(countIndex,1,new QTableWidgetItem(QString::number(correct_pose.y , 'f', 6 )));
    ui->tableWidget_base_pose->setItem(countIndex,2,new QTableWidgetItem(QString::number(correct_pose.z , 'f', 6 )));
    ui->tableWidget_base_pose->setItem(countIndex,3,new QTableWidgetItem(QString::number(correct_pose.rx , 'f', 6 )));
    ui->tableWidget_base_pose->setItem(countIndex,4,new QTableWidgetItem(QString::number(correct_pose.ry , 'f', 6 )));
    ui->tableWidget_base_pose->setItem(countIndex,5,new QTableWidgetItem(QString::number(correct_pose.rz , 'f', 6 )));

    calibration_.add_station_reference_pose(station_id, correct_pose);

    light_->setLuminance2(0);
}

void MainWindow::on_pushButton_moveto_base_pose_pressed()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    QString station_id = ui->comboBox_station_name_matrix->currentText();
    int row_count = ui->tableWidget_reference_pose->rowCount();

    if(row_count < 1){
        QMessageBox::critical(this, "错误\t", "请先设置参考位姿\t");
        return;
    }

    //QString pose_name = ui->tableWidget_robotarm_station_pos->item(current_index,0)->text();

    RobotPoseXYZ pose = {
        ui->tableWidget_base_pose->item(0,0)->text().toDouble(),
        ui->tableWidget_base_pose->item(0,1)->text().toDouble(),
        ui->tableWidget_base_pose->item(0,2)->text().toDouble(),
        ui->tableWidget_base_pose->item(0,3)->text().toDouble(),
        ui->tableWidget_base_pose->item(0,4)->text().toDouble(),
        ui->tableWidget_base_pose->item(0,5)->text().toDouble()
    };

    mainthread_robot_arm_->move_to_pose(pose,
                                        MoveType::LINE,
                                        LineSpeed,LineACC,LineDEC,
                                        BlockMotion::UNLOCKED);

}

void MainWindow::on_pushButton_moveto_base_pose_released()
{
    if(!mainthread_robot_arm_->isConnected())
    {
        QMessageBox::critical(this, "错误\t", "未连接到机械臂！\t");
        return;
    }

    mainthread_robot_arm_->stop_movement();
}

void MainWindow::on_pushButton_modbus_write_67_clicked()
{
    int value = ui->spinBox_modbus_coil_value_67->value();

    NDC_Client_.write_ADV_data_67(value);
}



/*****************
      等待信号
******************/

//等待取生料请求信号,下一阶段放到二次定位平台
bool MainWindow::waitfor_taking_raw_signal(int time){

    int value = 0;

    QTime outTime;

    if(time >= 0){
       outTime = QTime::currentTime().addMSecs(time);
    }

    while(1){

        bool state = NDC_Client_.read_AGV_State();
        state = NDC_Client_.Func_allow_take_raw_workpiece_from_lifter(value);
        if(state == true && value == 1){
            qDebug() << "waitfor_taking_raw_signal true";
            return true;
        }

        QTime dieTime = QTime::currentTime().addMSecs(500);
        while( QTime::currentTime() < dieTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        }

        //检测超时
        if( time >= 0 && QTime::currentTime() > outTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

            //qDebug() << "waitfor_taking_raw_signal timeout";
            return false;
        }

        //检测切换到手动模式
        if(!mainthread_robot_arm_->is_remote_mode()){
            stop_work_flow();
            QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
            return false;
        }
    }

   return true;
}

//等待取加工料请求信号
bool MainWindow::waitfor_taking_workpiece_signal(int time){

    int value = 0;

    QTime outTime;

    if(time >= 0){
        outTime = QTime::currentTime().addMSecs(time);
    }


    while(1){

        bool state = NDC_Client_.read_AGV_State();
        state = NDC_Client_.Func_allow_take_workpiece_from_CNC(value);
        state = NDC_Client_.Func_get_station_id(current_station_id);

        if(state == true && value == 1){

            if(current_station_id > 0){
                 return true;
            }
        }

        QTime dieTime = QTime::currentTime().addMSecs(500);
        while( QTime::currentTime() < dieTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        }

        //检测超时
        if( time >= 0 && QTime::currentTime() > outTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
             return false;
        }

        //检测切换到手动模式
        if(!mainthread_robot_arm_->is_remote_mode()){
            stop_work_flow();
            QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
            return false;
        }
    }

   return true;
}

//等待转移平台允许信号
bool MainWindow::waitfor_transfer_allowing_signal(int time){


    int value = 0;

    //设置超时时间
    QTime outTime;

    if(time >= 0){
         outTime  = QTime::currentTime().addMSecs(time);
    }


    while(1){

        //检测转移平台不可移动
        bool state = NDC_Client_.Func_allow_take_raw_workpiece_from_lifter(value);
        if(state == 1 && value == 1){
            return true;
        }

        QTime dieTime = QTime::currentTime().addMSecs(500);
        while( QTime::currentTime() < dieTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        }

        //检测超时
        if( time >= 0 && QTime::currentTime() > outTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
             return false;
        }

        //检测切换到手动模式
        if(!mainthread_robot_arm_->is_remote_mode()){
            stop_work_flow();
            QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
            return false;
        }
    }


       return true;
}

//等待开门信号(光电检测是否已经开门),检测到开门之后还要等几秒
bool MainWindow::waitfor_opening_CNC_door(int time){

    int value = 0;

    QTime outTime;

    if(time >= 0){
        outTime = QTime::currentTime().addMSecs(time);
    }


    while(1){

        //

        //监测光电感应信号，读取3次
        int door_closed = 0;
         bool receive = mainthread_robot_arm_->get_DI(6,door_closed);

         sleep_ms(500);
         int door_closed_2 = 0;
         bool receive_2 = mainthread_robot_arm_->get_DI(6,door_closed_2);

         sleep_ms(500);
         int door_closed_3 = 0;
         bool receive_3 = mainthread_robot_arm_->get_DI(6,door_closed_3);


         if(receive == true
            && receive_2 == true
            && receive_3 == true
            && door_closed == door_closed_2
            && door_closed == door_closed_3){
             //then
         }
         else{
             qDebug() << "light signal do not stable";
             continue;
         }

        if(receive == true && !door_closed){

            qDebug() << "waitfor_opening_CNC_door true";
            return true;
        }
        else if (receive == true && door_closed){
           qDebug() << "waitfor_opening_CNC_door false";
        }

        QTime dieTime = QTime::currentTime().addMSecs(100);
        while( QTime::currentTime() < dieTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        }

        //检测超时
        if( time >= 0 && QTime::currentTime() > outTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
            qDebug() << "waitfor_opening_CNC_door timeout";
            return false;
        }

        //检测切换到手动模式
        if(!mainthread_robot_arm_->is_remote_mode()){
            stop_work_flow();
            QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
            return false;
        }
    }

       return true;
}

//等待放料请求信号（放料时的开门信号）
bool MainWindow::waitfor_putting_raw_signal(int time){

    int value = 0;
    QTime outTime;
    if(time >= 0){
       outTime = QTime::currentTime().addMSecs(time);
    }


    while(1){

        //bool state = NDC_Client_.read_AGV_State();
        //state = NDC_Client_.Func_allow_put_raw_workpiece_to_CNC(value);
        int status = -1;
        QString station_qr_info = station_id_mapper.at(current_station_id);
        bool state = http_client_.operate_get_status(station_qr_info,status);

        //监测光电感应信号
        int door_closed = 0;
         bool receive = mainthread_robot_arm_->get_DI(6,door_closed);


        if(state == true && status == 0&& receive == true && !door_closed){
            //NDC_Client_.write_AGV_data(53,0);
            qDebug() << "waitfor_putting_raw_signal true";
            return true;
        }
        else if(state == true && status == 0&& receive == true && door_closed){
            //NDC_Client_.write_AGV_data(53,0);
            //qDebug() << "waitfor_putting_raw_signal true";
            Sleep(5000);
            receive = mainthread_robot_arm_->get_DI(6,door_closed);
            if(state == true && status == 0&& receive == true && !door_closed){
                //NDC_Client_.write_AGV_data(53,0);
                qDebug() << "waitfor_putting_raw_signal true";
                Sleep(2000);
                return true;
            }
            return false;
        }

        QTime dieTime = QTime::currentTime().addMSecs(500);
        while( QTime::currentTime() < dieTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        }

        //检测超时
        if(time >= 0 && QTime::currentTime() > outTime ){
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

            //qDebug() << "waitfor_putting_raw_signal timeout";
            return false;
        }

        //检测切换到手动模式
        if(!mainthread_robot_arm_->is_remote_mode()){
            stop_work_flow();
            QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
            return false;
        }
    }
       return true;
}

/*****************
      发送信号
******************/

//给转移平台允许转移信号
bool MainWindow::send_transfer_allowing_signal(){

    //复位掉可取料信号
    //NDC_Client_.write_AGV_data(55,0);

    bool state =  NDC_Client_.write_ADV_data_66(1);

    sleep_ms(500);
    state  =  NDC_Client_.write_ADV_data_67(1);

    return state;
}
//给AGV取料完成信号
bool MainWindow::send_finishing_taking_signal(){

    //NDC_Client_.write_AGV_data(54,0);

    QString station_qr_info = station_id_mapper.at(current_station_id);
    int error = -1;
    while(1){

            bool state = http_client_.operate_blow(station_qr_info,error);

            if(state == true){
                break;
            }

            //检测切换到手动模式
            if(!mainthread_robot_arm_->is_remote_mode()){
                stop_work_flow();
                QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
                return false;
            }
    }

    bool state =  NDC_Client_.write_ADV_data_64(1);


    return true;

}
//给AGV放料完成信号
bool MainWindow::send_finishing_putting_signal(){

    //return state;

    QString station_qr_info = station_id_mapper.at(current_station_id);
    int error = -1;
    while(1){
            bool state = http_client_.operate_close_door(station_qr_info, error);

            if(state == true){
                break;
            }

            //检测切换到手动模式
            if(!mainthread_robot_arm_->is_remote_mode()){
                stop_work_flow();
                QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
                return false;
            }
    }

    bool state =  NDC_Client_.write_ADV_data_65(1);

    return true;
}

bool MainWindow::send_dm_message(QString dm_message,int outtime)
{

   QTime outTime;

   if(time >= 0){
      outTime = QTime::currentTime().addMSecs(outtime);
   }

    QString station_qr_info = station_id_mapper.at(current_station_id);
    int error = -1;
    while(1){

            bool state = http_client_.upload_dm_message(station_qr_info,dm_message);

            if(state == true){
                break;
            }

            //检测切换到手动模式
            if(!mainthread_robot_arm_->is_remote_mode()){
                stop_work_flow();
                QMessageBox::information(this, "提示\t", "机械臂已切换到手动模式\t");
                return false;
            }

            QTime dieTime = QTime::currentTime().addMSecs(500);
            while( QTime::currentTime() < dieTime ){
                QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
            }

            //检测超时
            if( time >= 0 && QTime::currentTime() > outTime ){
                QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

                //qDebug() << "waitfor_taking_raw_signal timeout";
                return false;
            }

    }

    return true;
}


/*****************
     机械臂移动任务
******************/

//从待机点，移动到转移平台前方，（不分左右）
bool MainWindow::moveto_transfer_front_pose_from_initialpose(){


    //待机点检测
 //  bool state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
 //                                              MoveType::JOINT,
 //                                              JointSpeed,JointACC,JointDEC,
 //                                              BlockMotion::LOCKED);
 //  if(state == false){
 //      stop_work_flow();
 //      return false;
 //  }

  //  state = mainthread_robot_arm_->move_to_common_pose("转移平台前方上方",
  //                                             MoveType::JOINT,
  //                                             JointSpeed,JointACC,JointDEC,
  //                                             BlockMotion::LOCKED);

  //  if(state == false){
  //      stop_work_flow();
  //      return false;
  //  }

    //待机点检测
     bool  state = mainthread_robot_arm_->move_to_common_pose("转移平台前方",
                                               MoveType::JOINT,
                                               JointSpeed,JointACC,JointDEC,
                                               BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

   return state;
}
//从转移平台前方，移动到取生料位，（不分左右）
bool MainWindow::moveto_tranferplane_pose(){

    //移动到取生料点上方
   bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
                                               MoveType::LINE,
                                               LineSpeed,LineACC,LineDEC,
                                               BlockMotion::LOCKED);

   if(state == false){
       stop_work_flow();
       return false;
   }

   //移动到取生料点
  state = mainthread_robot_arm_->move_to_common_pose("取生料点",
                                              MoveType::LINE,
                                              15,15,15,
                                              BlockMotion::LOCKED);

  if(state == false){
      stop_work_flow();
      return false;
  }


    return state;
}

//从转移平台前方，移动到放熟料位，（不分左右）
bool MainWindow::moveto_tranferplane_unload_pose(){
    //移动到取生料点上方
//   bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
//                                               MoveType::LINE,
//                                               LineSpeed,LineACC,LineDEC,
//                                               BlockMotion::LOCKED);

   bool state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                               MoveType::LINE,
                                               LineSpeed,
                                               10);

   if(state == false){
       stop_work_flow();
       return false;
   }

   //移动到取生料点
//  state = mainthread_robot_arm_->move_to_common_pose("放熟料点",
//                                              MoveType::LINE,
//                                              15,15,15,
//                                              BlockMotion::LOCKED);

  state = mainthread_robot_arm_->add_common_waypoint("放熟料点",
                                                 MoveType::LINE,
                                                 15,
                                                 10);
  if(state == false){
      stop_work_flow();
      return false;
  }

  state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

  if(state == false){
      stop_work_flow();
      return false;
  }

    return state;
}

//从取生料位，移动到二次定位平台上方,（不分左右）
bool MainWindow::moveto_locationplane_pose_above_from_transfer(){

    bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
                                               MoveType::LINE,
                                               LineSpeed,LineACC,LineDEC,
                                               BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //真空检测
    //sleep_ms(1000);
    int suck_ok = 0;
    bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

     //吸生料异常处理
    if(receive == false || suck_ok == 0){

        //关闭吸真空
        mainthread_robot_arm_->set_DO(6,0);
        exception_when_taking_raw_at_transfer();
        stop_work_flow();
        QMessageBox::information(this, "提示\t", "转移平台吸生料异常\t");
        return false;
    }

//     state = mainthread_robot_arm_->move_to_common_pose("转移平台前方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

     if(state == false){
         stop_work_flow();
         return false;
     }


    //移动到二次定位平台取料点上方
//    state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点上方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //移动到二次定位平台取料点
//    state = mainthread_robot_arm_->move_to_common_pose("二次平台放料点",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("二次平台放料点",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);


    if(state == false){
        stop_work_flow();
        return false;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    return state;
}
//从二次平台上方，移动到吹DM码上方（不分左右）
bool MainWindow::moveto_chowing_DMcode_pose_above(){

    bool state = mainthread_robot_arm_->move_to_common_pose("吹DM点",
                                                       MoveType::LINE,
                                                       LineSpeed,LineACC,LineDEC,
                                                       BlockMotion::LOCKED);


    if(state == false){
        stop_work_flow();
        return false;
    }

       return state;
}
//从二次定位平台，移动回待机点（不分左右）
bool MainWindow::moveto_initialpose_from_locationplane(){


    //移动到二次定位平台取料点上方
//    bool state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点上方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false){
        stop_work_flow();
        return false;
    }


    //移动回二次平台上方待机点
//    state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                       MoveType::JOINT,
//                                                       JointSpeed,JointACC,JointDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false){
        stop_work_flow();
        return false;
    }


    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }


    return state;
}
//从转移平台取放料点，移动到待机点
bool MainWindow::moveto_initialpose_from_transfer(){

    //移动到取生料点上方
//    bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    bool state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);



    if(state == false){
        stop_work_flow();
        return false;
    }


    //移动到转移平台前方
//    state = mainthread_robot_arm_->move_to_common_pose("转移平台前方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //移动回初始化待机点
//    state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                       MoveType::JOINT,
//                                                       JointSpeed,JointACC,JointDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                    MoveType::JOINT,
                                                    JointSpeed,
                                                    10);

    if(state == false){
        stop_work_flow();
        return false;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    return state;
}


//上下移动并清洗（不分左右）
bool MainWindow::move_upanddown(){


    RobotPoseJoint pose_j;
    mainthread_robot_arm_->getRobotCommonPose("洗料池", pose_j);

//    qDebug() << "pose_j " << pose_j.joint1;
//    qDebug() << "pose_j " << pose_j.joint2;
//    qDebug() << "pose_j " << pose_j.joint3;
//    qDebug() << "pose_j " << pose_j.joint4;
//    qDebug() << "pose_j " << pose_j.joint5;
//    qDebug() << "pose_j " << pose_j.joint6;

    RobotPoseXYZ pose_xyz;
    mainthread_robot_arm_->getFK(pose_j,pose_xyz);

//    qDebug() << "pose_xyz " << pose_xyz.x;
//    qDebug() << "pose_xyz " << pose_xyz.y;
//    qDebug() << "pose_xyz " << pose_xyz.z;
//    qDebug() << "pose_xyz " << pose_xyz.rx;
//    qDebug() << "pose_xyz " << pose_xyz.ry;
//    qDebug() << "pose_xyz " << pose_xyz.rz;

    bool state;


    for(int i=0; i<3; i++){
        //移动到洗料池上方
//        bool state = mainthread_robot_arm_->move_to_pose(pose_xyz,
//                                                         MoveType::LINE,
//                                                         LineSpeed,LineACC,LineDEC,
//                                                         BlockMotion::LOCKED);

       state = mainthread_robot_arm_->add_xyz_waypoint(pose_xyz,
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);


        if(state == false){
            stop_work_flow();
            return false;
        }

        RobotPoseXYZ temp_pose = pose_xyz;
        temp_pose.z -= 100;
        //移动到洗料池上方
//        state = mainthread_robot_arm_->move_to_pose(temp_pose,
//                                                         MoveType::LINE,
//                                                         LineSpeed,LineACC,LineDEC,
//                                                         BlockMotion::LOCKED);

        state = mainthread_robot_arm_->add_xyz_waypoint(temp_pose,
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false){
            stop_work_flow();
            return false;
        }
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    return true;
}
//从水槽，移动到放OK料前方（同取生料位上方）（不分左右）
bool MainWindow::moveto_transfer_front_pose_from_waterpool(){

    //移动到
    bool state = mainthread_robot_arm_->move_to_common_pose("洗料池上方",
                                                        MoveType::LINE,
                                                        LineSpeed,LineACC,LineDEC,
                                                        BlockMotion::LOCKED);

//    bool state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
//                                                MoveType::LINE,
//                                                LineSpeed,
//                                                10);
    if(state == false){
        stop_work_flow();
        return false;
    }

    //移动到
    state = mainthread_robot_arm_->move_to_common_pose("洗料池上方沥水点",
                                                        MoveType::LINE,
                                                        LineSpeed,LineACC,LineDEC,
                                                        BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }
    sleep_ms(5000);

//     state = mainthread_robot_arm_->move_to_common_pose("左边机台去水槽上方",
//                                                        MoveType::JOINT,
//                                                        JointSpeed,JointACC,JointDEC,
//                                                        BlockMotion::LOCKED);

      state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                                 MoveType::JOINT,
                                                 JointSpeed,
                                                 10);
     if(state == false){
         stop_work_flow();
         return false;
     }


//     state = mainthread_robot_arm_->move_to_common_pose("转移平台前方",
//                                                        MoveType::JOINT,
//                                                        JointSpeed,JointACC,JointDEC,
//                                                        BlockMotion::LOCKED);

     state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                 MoveType::JOINT,
                                                 JointSpeed,
                                                 10);

     if(state == false){
         stop_work_flow();
         return false;
     }

     state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

     if(state == false){
         stop_work_flow();
         return false;
     }

       return state;
}


//从待机点，移动到视觉拍照位(分左右)
bool MainWindow::moveto_taking_image_pose(){

    //qDebug() << "current_station_id " << current_station_id;

    QString type = calibration_.get_station_type(current_station_id);
    //qDebug() << "type " << type;

    if(type == "左机台"){

        //移动到
//        bool state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                            MoveType::JOINT,
//                                                            JointSpeed,JointACC,JointDEC,
//                                                            BlockMotion::LOCKED);


        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                    MoveType::JOINT,
                                                    JointSpeed,
                                                    10);


        if(state == false){
            stop_work_flow();
            return false;
        }


//      state = mainthread_robot_arm_->move_to_common_pose("左边拍照待机点二",
//                                                                  MoveType::LINE,
//                                                                  LineSpeed,LineACC,LineDEC,
//                                                                  BlockMotion::LOCKED);


      state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点二",
                                                  MoveType::LINE,
                                                  LineSpeed,
                                                  10);

      if(state == false){
          stop_work_flow();
          return false;
      }

      state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

      if(state == false){
          stop_work_flow();
          return false;
      }

        //移动右边拍照待机点
        state = mainthread_robot_arm_->move_to_station_pose(current_station_id,
                                                            "相机拍照点",
                                                            MoveType::LINE,
                                                            LineSpeed,LineACC,LineDEC,
                                                            BlockMotion::LOCKED);

//        state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
//                                                            "相机拍照点",
//                                                            MoveType::LINE,
//                                                            LineSpeed,
//                                                            10);

        if(state == false){
            stop_work_flow();
            return false;
        }

        return state;
    }
    else if(type == "右机台")
    {
        //移动右边拍照待机点
          bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,JointACC,JointDEC,
                                                            BlockMotion::LOCKED);

//          bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
//                                                      MoveType::JOINT,
//                                                      JointSpeed,
//                                                      10);

          if(state == false){
              stop_work_flow();
              return false;
          }

        //移动右边拍照待机点
        state = mainthread_robot_arm_->move_to_station_pose(current_station_id,
                                                            "相机拍照点",
                                                            MoveType::LINE,
                                                            LineSpeed,LineACC,LineDEC,
                                                            BlockMotion::LOCKED);

//        state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
//                                                            "相机拍照点",
//                                                            MoveType::LINE,
//                                                            LineSpeed,
//                                                            10);


        if(state == false){
            stop_work_flow();
            return false;
        }

        //state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        return state;
    }

    //视觉拍照前，先关闭空压机
    NDC_Client_.write_AGV_data_68(1);

}
//从视觉定位处，移动到CNC机台门口（分左右）
bool MainWindow::moveto_CNC_front_door_from_visionpose(){

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

//        bool state = mainthread_robot_arm_->move_to_common_pose("左边拍照待机点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

        bool state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点",
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false){
            stop_work_flow();
            return false;
        }

//         state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false){
             stop_work_flow();
             return false;
         }

//         state = mainthread_robot_arm_->move_to_common_pose("左边门前点二",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false){
             stop_work_flow();
             return false;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false){
             stop_work_flow();
             return false;
         }

         return state;
    }
    else if(type == "右机台"){

//        bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

        bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);

        if(state == false){
            stop_work_flow();
            return false;
        }

//         state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false){
             stop_work_flow();
             return false;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false){
             stop_work_flow();
             return false;
         }

         return state;
    }
       return false;
}
//从机台门口，移动到CNC机台OK料位（可能分左右）
bool MainWindow::moveto_CNC_baseplane_taking_pose(){

    //qDebug() << "moveto_CNC_baseplane_pose ";
    //张开小夹爪
    open_jaw();


//    qDebug() << "station_correct_pose " << station_correct_pose.x;
//    qDebug() << "station_correct_pose " << station_correct_pose.y;
//    qDebug() << "station_correct_pose " << station_correct_pose.z;
//    qDebug() << "station_correct_pose " << station_correct_pose.rx;
//    qDebug() << "station_correct_pose " << station_correct_pose.ry;
//    qDebug() << "station_correct_pose " << station_correct_pose.rz;

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    RobotPoseXYZ station_correct_pose_take_finished = station_correct_pose;

        station_correct_pose_take_finished.z -= 5;//mm

    //移动到CNC机台OK料位
    state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
                                                MoveType::LINE,
                                                30,10,10,
                                                BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //关闭小夹爪
    close_jaw();


       return state;
}

//从机台门口，移动到CNC机台OK料位（可能分左右）
bool MainWindow::moveto_CNC_baseplane_putting_pose(){

    //张开小夹爪
    //open_jaw();


//    qDebug() << "station_correct_pose " << station_correct_pose.x;
//    qDebug() << "station_correct_pose " << station_correct_pose.y;
//    qDebug() << "station_correct_pose " << station_correct_pose.z;
//    qDebug() << "station_correct_pose " << station_correct_pose.rx;
//    qDebug() << "station_correct_pose " << station_correct_pose.ry;
//    qDebug() << "station_correct_pose " << station_correct_pose.rz;

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //RobotPoseXYZ station_correct_pose_take_finished = station_correct_pose;

    //station_correct_pose_take_finished.z -= 5;//mm

    //移动到CNC机台OK料位
//    state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
//                                                MoveType::LINE,
//                                                50,15,15,
//                                                BlockMotion::LOCKED);

//    if(state == false){
//        stop_work_flow();
//        return false;
//    }

    //关闭小夹爪
    //close_jaw();

    return state;
}




//从CNC机台，移动到水槽（分左右）
bool MainWindow::moveto_waterpool(){

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                80,40,40,
                                                BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //再做一次真空检测
        int suck_ok = 0;
        bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);
        if(receive == false ){
            while (1) {
                Sleep(1000);
                receive = mainthread_robot_arm_->get_DI(10, suck_ok);
               if(receive == true ){break;}
            }

        }
        if( suck_ok == 0){
            bool state = chow_workpiece();
            exception_when_taking_workpiece();

            stop_work_flow();
            return false;
        }


    //移动到丢废料点
    state = mainthread_robot_arm_->move_to_pose(station_correct_discard_pose,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    //打开夹爪,丢弃废料
    state = open_jaw();

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);


        if(state == false){
            stop_work_flow();
            return false;
        }

        //发送取料完成信号
        state = send_finishing_taking_signal();

        if(state == false){
            stop_work_flow();
            return false;
        }

        //打开抽水泵(
         log_message(LogType::LOG_INFO,"打开抽水泵");
        start_washing_workpiece();

//         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                          MoveType::JOINT,
//                                                          JointSpeed,JointACC,JointDEC,
//                                                          BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                             MoveType::JOINT,
                                                             JointSpeed,
                                                             10);

         if(state == false){
             stop_work_flow();
             return false;
         }

//         state = mainthread_robot_arm_->move_to_common_pose("左边机台去水槽上方",
//                                                          MoveType::JOINT,
//                                                          JointSpeed,JointACC,JointDEC,
//                                                          BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                                             MoveType::JOINT,
                                                             JointSpeed,
                                                             10);

         if(state == false){
             stop_work_flow();
             return false;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false){
             stop_work_flow();
             return false;
         }

    }
    else if(type == "右机台"){
        state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);

        if(state == false){
            stop_work_flow();
            return false;
        }
    }


    //发送取料完成信号
    state = send_finishing_taking_signal();

    if(state == false){
        stop_work_flow();
        return false;
    }

    //打开抽水泵(
     log_message(LogType::LOG_INFO,"打开抽水泵");
    start_washing_workpiece();


//    state = mainthread_robot_arm_->move_to_common_pose("洗料池上方",
//                                                     MoveType::JOINT,
//                                                     JointSpeed,JointACC,JointDEC,
//                                                     BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
                                                        MoveType::JOINT,
                                                        JointSpeed,
                                                        10);

    if(state == false){
        stop_work_flow();
        return false;
    }

//    state = mainthread_robot_arm_->move_to_common_pose("洗料池",
//                                                     MoveType::LINE,
//                                                     LineSpeed,LineACC,LineDEC,
//                                                     BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("洗料池",
                                                        MoveType::LINE,
                                                        LineSpeed,
                                                        10);

    if(state == false){
        stop_work_flow();
        return false;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    return state;
}
//从视觉定位处，移动到二次定位平台(分左右)
bool MainWindow::moveto_locationplane_pose_from_visionpose(){

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){
        //移动左边拍照待机点
        bool state = mainthread_robot_arm_->move_to_common_pose("左边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,LineACC,LineDEC,
                                                            BlockMotion::LOCKED);


        if(state == false){
            stop_work_flow();
            return false;
        }

      //  state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
      //                                                     MoveType::JOINT,
      //                                                      JointSpeed,JointACC,JointDEC,
      //                                                      BlockMotion::LOCKED);

         //return state;
    }
    else if(type == "右机台"){
        //移动右边拍照待机点
//        bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
//                                                            MoveType::LINE,
//                                                            LineSpeed,LineACC,LineDEC,
//                                                            BlockMotion::LOCKED);

        bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);
        if(state == false){
            stop_work_flow();
            return false;
        }

//        state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                            MoveType::JOINT,
//                                                            JointSpeed,JointACC,JointDEC,
//                                                            BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,
                                                            10);

        if(state == false){
            stop_work_flow();
            return false;
        }

        state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        if(state == false){
            stop_work_flow();
            return false;
        }

         //return state;
    }


//    bool state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点上方",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                       MoveType::LINE,
                                                       LineSpeed,
                                                       10);

    if(state == false){
        stop_work_flow();
        return false;
    }

//    state = mainthread_robot_arm_->move_to_common_pose("吸二次定位平台生料上方点",
//                                                       MoveType::LINE,
//                                                       LineSpeed,LineACC,LineDEC,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("吸二次定位平台生料上方点",
                                                           MoveType::LINE,
                                                           LineSpeed,
                                                           10);

    if(state == false){
        stop_work_flow();
        return false;
    }

//    state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点",
//                                                       MoveType::LINE,
//                                                       15,15,15,
//                                                       BlockMotion::LOCKED);

    state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点",
                                                           MoveType::LINE,
                                                           15,
                                                           10);

    if(state == false){
        stop_work_flow();
        return false;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false){
        stop_work_flow();
        return false;
    }

    return state;



}
//从二次定位平台，移动到CNC机台门口（分左右）
bool MainWindow::moveto_CNC_front_door_from_locationplane(){


    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

//        bool state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点上方",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                        MoveType::LINE,
                                                                        LineSpeed,
                                                                        10);
        if(state == false){
            stop_work_flow();
            return false;
        }


        state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);
        if(state == false){
            stop_work_flow();
            return false;
        }
//         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

          state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

         if(state == false){
             stop_work_flow();
             return false;
         }
//         state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
//                                                           MoveType::JOINT,
//                                                           JointSpeed,JointACC,JointDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                               MoveType::JOINT,
                                                               JointSpeed,
                                                               10);
         if(state == false){
             stop_work_flow();
             return false;
         }
//         state = mainthread_robot_arm_->move_to_common_pose("左边门前点二",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);
         if(state == false){
             stop_work_flow();
             return false;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false){
             stop_work_flow();
             return false;
         }

         return state;
    }
    else if(type == "右机台"){

//        bool state = mainthread_robot_arm_->move_to_common_pose("二次平台取料点上方",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                        MoveType::LINE,
                                                                        LineSpeed,
                                                                        10);
        if(state == false){
            stop_work_flow();
            return false;
        }


        state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                              MoveType::LINE,
                                                              LineSpeed,
                                                              10);
        if(state == false){
            stop_work_flow();
            return false;
        }

//         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
//                                                           MoveType::LINE,
//                                                           LineSpeed,LineACC,LineDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);

         if(state == false){
             stop_work_flow();
             return false;
         }
//         state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
//                                                           MoveType::JOINT,
//                                                           JointSpeed,JointACC,JointDEC,
//                                                           BlockMotion::LOCKED);

         state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                               MoveType::JOINT,
                                                               JointSpeed,
                                                               10);
         if(state == false){
             stop_work_flow();
             return false;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false){
             stop_work_flow();
             return false;
         }

         return state;
    }


}
//从CNC机台，移动回待机点（分左右）
bool MainWindow::moveto_initialpose_from_CNC(){


    bool state;

    state = mainthread_robot_arm_->set_DO(6,0);
    state = mainthread_robot_arm_->set_DO(3,1);


    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                120,50,50,
                                                BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }
    //关闭吹料
    state = mainthread_robot_arm_->set_DO(3,0);


    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

        //发送放料完成信号
        state = send_finishing_putting_signal();

        if(state == false){
            stop_work_flow();
            return false;
        }

         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
         return state;

    }
    else if(type == "右机台"){
        state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

        //发送放料完成信号
        state = send_finishing_putting_signal();

        if(state == false){
            stop_work_flow();
            return false;
        }

         state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
         return state;
    }

}

//移动到丢废料点
bool MainWindow::moveto_discard_unused(){

    return true;
}


/****************************
     空压机、真空泵、抽水泵动作
****************************/

//吸料
bool MainWindow::suck_workpiece(){


    bool state;

    state = mainthread_robot_arm_->set_DO(6,1);

    if(state == false){
       return false;
    }

    //真空检测
    sleep_ms(1000);
    int suck_ok = 0;
    bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

    if(receive == false || suck_ok == 0){
        return false;
    }

       return true;
}
//吹料
bool MainWindow::chow_workpiece(){

    bool state;

    state = mainthread_robot_arm_->set_DO(6,0);
    state = mainthread_robot_arm_->set_DO(3,1);
    sleep_ms(1000);
    state = mainthread_robot_arm_->set_DO(3,0);

    if(state == false){
        return false;
    }

       return true;
}

//吹气n秒
bool MainWindow::chow_only(int time){

    bool state;

    state = mainthread_robot_arm_->set_DO(6,0);
    state = mainthread_robot_arm_->set_DO(3,1);
    sleep_ms(time);
    state = mainthread_robot_arm_->set_DO(3,0);

    if(state == false){
        return false;
    }

       return true;
}


//吹产品DM码
bool MainWindow::chow_DMcode(){

    //吹产品DM码
    bool state = mainthread_robot_arm_->set_DO(2,1);
    sleep_ms(1000);
    //关掉吹气
    state = mainthread_robot_arm_->set_DO(2,0);

       return true;
}

//打开抽水泵
bool MainWindow::start_washing_workpiece(){

    bool state = mainthread_robot_arm_->set_DO(5,1);

    return state;
}

//关闭抽水泵
bool MainWindow::close_washing_workpiece(){

    bool state = mainthread_robot_arm_->set_DO(5,0);

    return state;
}
//闭合小夹爪
bool MainWindow::close_jaw(){

    bool state = mainthread_robot_arm_->set_DO(4,1);

    sleep_ms(1000);

    int data;
    state = mainthread_robot_arm_->get_DI(11,data);

    if(state == true && data == 0){

        return true;
    }
    else{
        return false;
    }
}
//松开小夹爪
bool MainWindow::open_jaw(){

    bool state = mainthread_robot_arm_->set_DO(4,0);

    sleep_ms(1000);

    int data;
    state = mainthread_robot_arm_->get_DI(11,data);

    if(state == true && data == 1){

        return true;
    }
    else{
        return false;
    }
}

/*****************
     视觉处理
******************/

//视觉定位
bool MainWindow::process_image_and_correct_pose(){

    //打开光源
    //light_->setLuminance2(255);

    //设置曝光时间
    int exposure_time = calibration_.get_station_exposure_time(current_station_id);
    //qDebug() << current_station_id
    //         << " exposure_time " << exposure_time;

    camera_->set_exposure_time(CameraName::RobotArm,exposure_time);

    cv::Point2d center;
    float angle;

    //第1次拍照匹配
    bool state = false;

    //最多尝试3次匹配
    int try_time = 0;
    while(state == false && try_time < 3 ){

        //打开光源
        light_->setLuminance2(255);
        sleep_ms(3000);

        state = get_image_and_match(current_station_id,
                                         center,
                                         angle);
        try_time++;
    }

    if(state == false){
            return false;
    }


     current_match_angle = angle;
     current_match_center = center;

     //纠正放料点
     RobotPoseXYZ pose;
     state = mainthread_robot_arm_->getRobotStationPose(current_station_id,
                                                               "测试点",
                                                               pose);

     //qDebug() << "current_station_id " << current_station_id;
//     qDebug() << "pose " << pose.x;
//     qDebug() << "pose " << pose.y;
//     qDebug() << "pose " << pose.z;
//     qDebug() << "pose " << pose.rx;
//     qDebug() << "pose " << pose.ry;
//     qDebug() << "pose " << pose.rz;

     if(state == false) return false;

     RobotPoseXYZ correct_pose;
     state = calibration_.correct_station_pose(current_station_id,
                                       center,angle,
                                       pose,
                                       correct_pose);

//     qDebug() << "correct_pose " << correct_pose.x;
//     qDebug() << "correct_pose " << correct_pose.y;
//     qDebug() << "correct_pose " << correct_pose.z;
//     qDebug() << "correct_pose " << correct_pose.rx;
//     qDebug() << "correct_pose " << correct_pose.ry;
//     qDebug() << "correct_pose " << correct_pose.rz;

     station_correct_pose = correct_pose;

     //偏移量
     double offset_x = correct_pose.x - pose.x;
     double offset_y = correct_pose.y - pose.y;
     double offset_a = correct_pose.rz - pose.rz;

     ui->doubleSpinBox_x_offset->setValue(offset_x);
     ui->doubleSpinBox_y_offset->setValue(offset_y);
     ui->doubleSpinBox_a_offset->setValue(offset_a);


     //矫正丢废料点

     //RobotPoseXYZ pose;
     state = mainthread_robot_arm_->getRobotStationPose(current_station_id,
                                                               "丢废料点",
                                                               pose);


//     qDebug() << "current_station_id " << current_station_id;
//     qDebug() << "pose " << pose.x;
//     qDebug() << "pose " << pose.y;
//     qDebug() << "pose " << pose.z;
//     qDebug() << "pose " << pose.rx;
//     qDebug() << "pose " << pose.ry;
//     qDebug() << "pose " << pose.rz;


     if(state == false) return false;

     state = calibration_.correct_station_pose(current_station_id,
                                       center,angle,
                                       pose,
                                       correct_pose);

//     qDebug() << "correct_pose " << correct_pose.x;
//     qDebug() << "correct_pose " << correct_pose.y;
//     qDebug() << "correct_pose " << correct_pose.z;
//     qDebug() << "correct_pose " << correct_pose.rx;
//     qDebug() << "correct_pose " << correct_pose.ry;
//     qDebug() << "correct_pose " << correct_pose.rz;

     station_correct_discard_pose = correct_pose;


     //关闭光源
     light_->setLuminance2(0);

     return true;
}
//读DM码
bool MainWindow::decode_DMcode(){

    //打开光源
    light_->setLuminance1(120);

    sleep_ms(500);

    if(!camera_->isOpen(CameraName::DataMatrix)){
        QMessageBox::information(this,"Open Camera ","Please Open Camera Fisrt!");
        return false;
    }

    cv::Mat image;
    camera_->GetImage(CameraName::DataMatrix, image);

    Dm_image = image.clone();
    cv::cvtColor(Dm_image,Dm_image,cv::COLOR_GRAY2RGB);

    //显示图像
    cv::Mat QTmat = image.clone();
    cv::cvtColor(QTmat,QTmat,cv::COLOR_GRAY2RGB);
    QImage qcolor = QImage((const unsigned char*)(QTmat.data),
                           QTmat.cols, QTmat.rows, QTmat.step,
                           QImage::Format_RGB888);

    int width = ui->label_data_matrix->width();
    int height = ui->label_data_matrix->height();

    QImage qcolorshow = qcolor.scaled(QTmat.cols, QTmat.rows)
                              .scaled(width, height,
                                      Qt::IgnoreAspectRatio,
                                      Qt::SmoothTransformation);

    ui->label_data_matrix->setPixmap(QPixmap::fromImage(qcolorshow));
    ui->label_data_matrix->resize(qcolorshow.size());

    //处理图像
    //ROI
    cv::Rect rect(876,172,1200,1200);
    cv::Mat Dm_image_roi = Dm_image(rect);

    //cv::imshow("ROI",Dm_image_roi);
    //cv::waitKey(1);

    //return;

    //Resize
    cv::Size size(Dm_image_roi.rows/2 ,Dm_image_roi.cols/2);
    cv::Mat resize_image;
    cv::resize(Dm_image_roi,resize_image,size);

    //cv::imshow("Resize",resize_image);
    //cv::waitKey(1);
    //return;

    //平滑
    cv::Mat blur_mat;
    cv::medianBlur(resize_image,blur_mat,5);
    //cv::imshow("blur_mat",blur_mat);
    //cv::waitKey(1);

    //二值化
    //cv::Mat threshold_image;
    //cv::threshold(resize_image,threshold_image,100,255,0);
    //cv::imshow("threshold",threshold_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/threshold.bmp",threshold_image);

    cv::Mat open_image;
    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));
    cv::morphologyEx(blur_mat,open_image,cv::MORPH_OPEN,element);
    //cv::imshow("open",open_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/open.bmp",open_image);

    cv::Mat dilate_image;
    cv::Mat element2 = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));//3,3
    cv::morphologyEx(open_image,dilate_image,cv::MORPH_ERODE,element2);
    //cv::imshow("dilate",dilate_image);
    //cv::waitKey(1);
    //cv::imwrite("C:/Users/OKAGV/Desktop/dilate.bmp",dilate_image);


    //转换为QImage
     qcolor = QImage((const unsigned char*)(dilate_image.data),
                           dilate_image.cols, dilate_image.rows, dilate_image.step,
                           QImage::Format_RGB888);


    QTime time;
    time.start();
    QImage img = qcolor;                 //globalImg2;
    if(img.isNull())
    {
        //QMessageBox::information(this,"信息","打开文件失败");
        return false;
    }
    else
    {
        DmtxMessage *msg;
        DmtxRegion *reg;
        QImage src =img.copy();
        DmtxImage *imgdtx;
        QPixmap pix=QPixmap::fromImage(src);
        QString str;
        int leftLineAngle;
        int bottomLineAngle;
        int cornerx;
        int cornery;

        //qDebug()<<"src.format() "<<src.format();

        if(src.format()==QImage::Format_Mono)
        {
        src=src.convertToFormat(QImage::Format_ARGB32);
        //qDebug()<<"转换后：src.format() "<<src.format();
        }

        int pack=DmtxPack32bppXRGB;
        switch(src.format())
            {
            case QImage::Format_ARGB32:
            pack=DmtxPack32bppXRGB;
            break;
            case QImage::Format_RGB32:
            pack=DmtxPack32bppXRGB;
            break;
            case QImage::Format_RGB888:
            pack=DmtxPack24bppRGB;
            break;
            case QImage::Format_Indexed8:
            pack=DmtxPack8bppK;
            break;
            case  QImage::Format_Grayscale8:
            pack=DmtxPack8bppK;
            }

        // 增加超时时间。
        DmtxTime beginTime = dmtxTimeNow();	// 根据系统设置情况，获得当前时间
        long timeout_ms = 2000;
        DmtxTime stopTime = dmtxTimeAdd(beginTime, timeout_ms);	// 增加xx ms

        imgdtx = dmtxImageCreate(src.bits(),src.width(),src.height(),pack);
        Q_ASSERT(imgdtx != NULL);

        DmtxDecode *dec = dmtxDecodeCreate(imgdtx, 1);
        Q_ASSERT(dec != NULL);

        reg = dmtxRegionFindNext(dec,&stopTime);	// 如果超时则认为没有找到
        // assert(reg != NULL);

        if(dmtxTimeExceeded(stopTime))
        {
            //qDebug()<<"检测DM码超时";
             //QMessageBox::information(this,"信息","decode timeout");
            light_->setLuminance1(0);

            QDateTime current = QDateTime::currentDateTimeUtc();
            std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
            std::string save_dir = "D:/DM_code/NG/" + datetime + ".png";
            //qDebug() << "save_dir " << QString::fromStdString(save_dir);
            //cv::imwrite(save_dir,image);

            return false;
        }
       else
        {
            if (reg != NULL)
            {
                msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
                if (msg != NULL)
                {
                    //                    cout << msg->output << endl;
                    //                    cout << msg->outputIdx << endl;

                    std::string strout = (char*)msg->output;//解码信息

                    //check DM code
                    if(strout.size() != 16){
                         return false;
                         qDebug() << "Error DM code";
                    }

                     //发送给NDC
//                    if(!strout.empty() && NDC_Client_.check_Modbus_Available()){
//                       NDC_Client_.write_DM_code_to_NDC(strout);
//                    }

                    dm_message = strout;

                    str=QString::fromStdString(strout);
                    QString outmsg=QString("<font color=\"#00FF00\">%1</font>").arg(QString::fromStdString(strout));
                    ui->textEdit->setText(outmsg);

                    DM_msg = QString::fromStdString(strout);

                    //  qDebug()<<"reg->leftAngle "<<reg->leftAngle;
                    //  qDebug()<<"reg->leftAngle "<<reg->bottomAngle;
                    //   qDebug()<<"reg->finalPos.X "<<reg->finalPos.X;
                    //   qDebug()<<"reg->finalPos.Y "<<reg->finalPos.Y;
                    //qDebug()<<"reg->leftLine.mag"<<reg->leftLine.mag;
                    //qDebug()<<"reg->leftLine.hOffset"<<reg->leftLine.hOffset;
                    //qDebug()<<"reg->leftLine.stepBeg"<<reg->leftLine.stepBeg;
                    //qDebug()<<"reg->leftLine.stepPos"<<reg->leftLine.stepPos;
                    //qDebug()<<"reg->leftLine.stepNeg"<<reg->leftLine.stepNeg;
                    //qDebug()<<"reg->leftLine.distSq"<<reg->leftLine.distSq;
                    //qDebug()<<"reg->leftLine.locBeg.X "<<reg->leftLine.locBeg.X;
                    //qDebug()<<"reg->leftLine.locBeg.Y "<<reg->leftLine.locBeg.Y;

//                    qDebug()<<"reg->leftLine.locPos.X "<<reg->leftLine.locPos.X;//准的
//                    qDebug()<<"reg->leftLine.locPos.Y "<<reg->leftLine.locPos.Y;//准的
//                    qDebug()<<"reg->leftLine.locNeg.X "<<reg->leftLine.locNeg.X;//准的
//                    qDebug()<<"reg->leftLine.locNeg.Y "<<reg->leftLine.locNeg.Y;//准的
//                    qDebug()<<"bottomLine.locPos.X "<<reg->bottomLine.locPos.X;
//                    qDebug()<<"bottomLine.locPos.Y "<<reg->bottomLine.locPos.Y;
//                    qDebug()<<"bottomLine.locNeg.X "<<reg->bottomLine.locNeg.X;
//                    qDebug()<<"bottomLine.locNeg.y "<<reg->bottomLine.locNeg.Y;

                    QPainter p;
                    p.begin(&pix);
                    p.setPen(QPen(Qt::blue, 2));
                    p.drawEllipse(reg->leftLine.locPos.X-5,pix.height()-reg->leftLine.locPos.Y-5, 10, 10);//准的
                    p.setPen(QPen(Qt::yellow, 2));
                    p.drawEllipse(reg->leftLine.locNeg.X-5,img.height()-reg->leftLine.locNeg.Y-5, 10, 10);//准的

                    p.setPen(QPen(Qt::darkRed, 2));
                    p.drawEllipse(reg->bottomLine.locPos.X-5,img.height()-reg->bottomLine.locPos.Y-5, 10, 10);//准的
                    p.setPen(QPen(Qt::darkGreen, 2));
                    p.drawEllipse(reg->bottomLine.locNeg.X-5,img.height()-reg->bottomLine.locNeg.Y-5, 10, 10);//准的

                    int lineOriginX=(reg->leftLine.locNeg.X+reg->bottomLine.locPos.X)/2;
                    int lineOriginY=img.height()-(reg->leftLine.locNeg.Y+reg->bottomLine.locPos.Y)/2;
                    int lineLeftX=reg->leftLine.locPos.X;
                    int lineLeftY=pix.height()-reg->leftLine.locPos.Y;
                    int lineBottomX=reg->bottomLine.locNeg.X;
                    int lineBottomY=img.height()-reg->bottomLine.locNeg.Y;

                    p.setPen(QPen(Qt::red, 2));
                    p.drawLine(lineOriginX,lineOriginY,lineLeftX,lineLeftY);
                    p.drawLine(lineOriginX,lineOriginY,lineBottomX,lineBottomY);
                    p.end();

                    leftLineAngle=reg->leftLine.angle;
                    bottomLineAngle=reg->bottomAngle;
                    cornerx=lineOriginX;
                    cornery=lineOriginY;

                    dmtxMessageDestroy(&msg);

                    QDateTime current = QDateTime::currentDateTimeUtc();
                    std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
                    std::string save_dir = "D:/DM_code/OK/" + datetime + ".png";
                    //qDebug() << "save_dir " << QString::fromStdString(save_dir);
                    //cv::imwrite(save_dir,image);


                }
                else
                {
                    //qDebug()<<"无法检测到2";
                    //QMessageBox::information(this,"信息","无法检测到2");
                    QDateTime current = QDateTime::currentDateTimeUtc();
                    std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
                    std::string save_dir = "D:/DM_code/NG/" + datetime + ".png";
                    //qDebug() << "save_dir " << QString::fromStdString(save_dir);
                    //cv::imwrite(save_dir,image);

                    return false;
                }
                dmtxRegionDestroy(&reg);
            }
            else
            {
                //qDebug()<<"无法检测到1";
                //QMessageBox::information(this,"信息","无法检测到1");
                QDateTime current = QDateTime::currentDateTimeUtc();
                std::string datetime = current.toString("yyyy-MM-dd-hh-mm-ss").toStdString();
                std::string save_dir = "D:/DM_code/NG/" + datetime + ".png";
                //qDebug() << "save_dir " << QString::fromStdString(save_dir);
                //cv::imwrite(save_dir,image);

                return false;
            }

        }

        dmtxDecodeDestroy(&dec);
        dmtxImageDestroy(&imgdtx);

        //qDebug()<<time.elapsed()/1000.0<<"s";

        ui->label_data_matrix->clear();
//        src=src.scaled(ui->label->width(),ui->label->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
//        ui->label->setPixmap(QPixmap::fromImage(src));//QImage 显示速度慢
        pix=pix.scaled(ui->label_data_matrix->width(),ui->label_data_matrix->height(),Qt::KeepAspectRatio);//Qt::KeepAspectRatio自适应大小，不变形
        ui->label_data_matrix->setPixmap(pix);

        ui->label_base->clear();
        QString base_str=str.left(8);
        QString label_base_text=QString("信息:左边 %1°底边 %2°角点X %3 角点Y %4 内容 %5..").
                arg(leftLineAngle).arg(bottomLineAngle).arg(cornerx).arg(cornery).arg(base_str);
        ui->label_base->setText(label_base_text);

        QString timestr= QString("解码耗时 ")
                        + QString::number(time.elapsed())
                        + QString(" ms");
        ui->label_time->setText(timestr);
    }


    light_->setLuminance1(0);

    return true;
}

void MainWindow::timer_work_flow_loop(){

         run_statemachine_flow();

        //run_all_work_flow();

        /*
        /////////////////////////////////////////////////////////
              //先从转移平台取生料


        //如果接到取生料请求
        if(waitfor_taking_raw_signal(1000)
           && has_workpeice_in_localization == false)
        {
            startTime = QTime::currentTime();
            //从初始化待机点到转移平台前方
           log_message(LogType::LOG_INFO,"从初始化待机点到转移平台前方");

            bool state = moveto_transfer_front_pose_from_initialpose();
            if(state == false) return;


             //移动到转移平台取生料位
             log_message(LogType::LOG_INFO,"移动到转移平台取生料位");
             state = moveto_tranferplane_pose();
             if(state == false) return;

             //吸料
              log_message(LogType::LOG_INFO,"吸料");
             state = mainthread_robot_arm_->set_DO(6,1);
             sleep_ms(1000);

             if(state == false) return;


             //从转移平台移动到二次定位平台放料点
             log_message(LogType::LOG_INFO,"从转移平台移动到二次定位平台放料点");
             state = moveto_locationplane_pose_above_from_transfer();
             if(state == false) return;

             //往上台之后，马上给吹料信号
             log_message(LogType::LOG_INFO,"往上台之后，马上给吹料信号");
             state = mainthread_robot_arm_->set_DO(6,0);
             state = mainthread_robot_arm_->set_DO(3,1);
             state = mainthread_robot_arm_->move_to_common_pose("二次平台放料点上方",
                                                                MoveType::LINE,
                                                                LineSpeed,LineACC,LineDEC,
                                                                BlockMotion::LOCKED);
             if(state == false){
                 stop_work_flow();
                 return;
             }


             //关闭吹料
             log_message(LogType::LOG_INFO,"关闭吹料");
             state = mainthread_robot_arm_->set_DO(3,0);

             //移动到吹DM码位置
             log_message(LogType::LOG_INFO,"移动到吹DM码位置");
             state = moveto_chowing_DMcode_pose_above();
             if(state == false) return;

             //吹DM码
             log_message(LogType::LOG_INFO,"吹DM码");
             state = chow_DMcode();
             //从二次定位平台移动回待机点
             log_message(LogType::LOG_INFO,"从二次定位平台移动回待机点");
             state = moveto_initialpose_from_locationplane();
             if(state == false) return;

             //循环，直到识别DM码
              log_message(LogType::LOG_INFO,"循环，直到识别DM码");
              int decode_time = 0;
             while(1){

                 if(decode_time >5){
                     NDC_Client_.write_AGV_data_69(1);
                     QMessageBox::information(this, "提示\t", "读取DM码失败，请更换二次定位平台生料\t");
                     decode_time = 0;
                     NDC_Client_.write_AGV_data_69(0);
                     continue;
                 }

                 //设置曝光时间
                 int exposure_time = 15000 + decode_time*5000;
                 camera_->set_exposure_time(CameraName::DataMatrix,exposure_time);

                 state = decode_DMcode();
                 if(state == true){
                     break;
                 }

                 if(stop_reading_dm == true){
                     break;
                     stop_reading_dm = false;
                 }

                 QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
                 decode_time++;
                 //break;
             }

             has_workpeice_in_localization = true;

        }else{

        }

        /////////////////////////////////////////
              //先进行从CNC取熟料


        //如果接到取CNC加工料的机台信号
        log_message(LogType::LOG_INFO,"如果接到取CNC加工料的机台信号");
        //qDebug() << "2";
        if(has_workpeice_in_localization == true
            && waitfor_taking_workpiece_signal(1000)){

            //移动到相机拍照位
            log_message(LogType::LOG_INFO,"移动到相机拍照位");
            bool state = moveto_taking_image_pose();
            if(state == false) return;

             //视觉定位
             log_message(LogType::LOG_INFO,"视觉定位");
             state = process_image_and_correct_pose();
             //视觉拍照异常处理流程
             if(state == false){
                 exception_when_processing_vision();
                 //stop_work_flow();
                 NDC_Client_.write_ADV_data_64(2);
                 sleep_ms(15000);

                 log_message(LogType::LOG_WARNING,
                             current_station_id+ "机台视觉拍照异常");
                 //QMessageBox::information(this, "提示\t", "视觉拍照异常\t");
                 return;
             }


             //拍照完，可以打开空压机
             log_message(LogType::LOG_INFO,"拍照完，可以打开空压机");
             NDC_Client_.write_AGV_data_68(0);
             //从视觉拍照点，移动到CNC机台门前方
             log_message(LogType::LOG_INFO,"从视觉拍照点，移动到CNC机台门前方");
             state = moveto_CNC_front_door_from_visionpose();
             if(state == false) return;

             //检测光电信号，大门是否打开
             log_message(LogType::LOG_INFO,"检测光电信号，大门是否打开");
             state = waitfor_opening_CNC_door(300000);
             if(state == false){
                 exception_when_opening_door();
                 stop_work_flow();
                 log_message(LogType::LOG_ERROR,
                             current_station_id+ "机台5分钟未开门");
                 QMessageBox::information(this, "提示\t", "CNC机台5分钟未开门\t");
                 return;
             }

             //移动到CNC内部取料位
             log_message(LogType::LOG_INFO,"移动到CNC内部取料位");
             state = moveto_CNC_baseplane_taking_pose();
             if(state == false) return;

             //吸料
             log_message(LogType::LOG_INFO,"吸料");
             state = mainthread_robot_arm_->set_DO(6,1);

             if(state == false) return;

             //真空检测
             log_message(LogType::LOG_INFO,"真空检测");
             sleep_ms(1000);
             //初始化状态
             bool has_workpeice_in_CNC = false;
             int suck_ok = 0;
             bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

             if(receive == false || suck_ok == 0){
                 has_workpeice_in_CNC = false;
                 //关闭吸真空
                 mainthread_robot_arm_->set_DO(6,0);
                 //return ;
             }
             else{
                 has_workpeice_in_CNC = true;
             }

             //吸到料，执行吸料后正常流程
             log_message(LogType::LOG_INFO,"吸到料，执行吸料后正常流程");
             if(has_workpeice_in_CNC == true){

                 //移动到水槽
                 log_message(LogType::LOG_INFO,"移动到水槽");
                 state = moveto_waterpool();
                 if(state == false) return;

                 //上下移动清洗
                 log_message(LogType::LOG_INFO,"上下移动清洗");
                 state = move_upanddown();
                 if(state == false) return;

                 //关闭抽水泵
                 log_message(LogType::LOG_INFO,"关闭抽水泵");
                 state = close_washing_workpiece();
                 //从水槽，移动到转移平台前方
                 log_message(LogType::LOG_INFO,"从水槽，移动到转移平台前方");
                 state = moveto_transfer_front_pose_from_waterpool();
                 if(state == false) return;

                 //等待转移平台放料允许信号
                 log_message(LogType::LOG_INFO,"等待转移平台放料允许信号");
                 state = waitfor_transfer_allowing_signal(-1);

                 //移动到转移平台放料位
                 log_message(LogType::LOG_INFO,"移动到转移平台放料位");
                 state = moveto_tranferplane_unload_pose();
                 if(state == false) return;

                 //吹料
                 log_message(LogType::LOG_INFO,"吹料");
                 state = chow_workpiece();

                 //从转移平台，移动到初始化待机点
                 log_message(LogType::LOG_INFO,"从转移平台，移动到初始化待机点");
                 state = moveto_initialpose_from_transfer();
                 if(state == false) return;

                 //发送转移平台允许移动信号
                 log_message(LogType::LOG_INFO,"发送转移平台允许移动信号");
                 state = send_transfer_allowing_signal();
             }
             else{

                 state = mainthread_robot_arm_->set_DO(6,0);

                 exception_when_no_workpiece_in_CNC();
             }

             ///////////////////////////////////////////////////////
                  //接着进行放生料到CNC


             //视觉拍照前，先关闭空压机
             log_message(LogType::LOG_INFO,"视觉拍照前，先关闭空压机");
             NDC_Client_.write_AGV_data_68(1);

             //移动到相机拍照位
              log_message(LogType::LOG_INFO,"移动到相机拍照位");
             state = moveto_taking_image_pose();
             if(state == false) return;

             //视觉定位
              log_message(LogType::LOG_INFO,"视觉定位");
             state = process_image_and_correct_pose();
             //视觉拍照异常处理流程
             if(state == false){
                 exception_when_processing_vision();
                 //NDC_Client_.write_ADV_data_64(2);
                 //sleep_ms(15000);
                 stop_work_flow();
                 log_message(LogType::LOG_ERROR,
                             current_station_id + "机台视觉拍照异常");
                 QMessageBox::information(this, "提示\t", "视觉拍照异常\t");
                 return;
             }


             //打开空压机
             log_message(LogType::LOG_INFO,"打开空压机");
             NDC_Client_.write_AGV_data_68(0);
             //从相机拍照位，移动到二次定位平台
             log_message(LogType::LOG_INFO,"从相机拍照位，移动到二次定位平台");
             state = moveto_locationplane_pose_from_visionpose();
             if(state == false) return;

             //吸料
             log_message(LogType::LOG_INFO,"吸料");
             state = suck_workpiece();
             //吸生料异常处理
             if(state == false){
                 //关闭吸真空
                 mainthread_robot_arm_->set_DO(6,0);
                 exception_when_taking_raw_at_localization();
                 stop_work_flow();
                 QMessageBox::information(this, "提示\t", "二次定位平台取生料异常\t");
                 return;
             }

             //从二次定位平台，移动到CNC机台门前方
             log_message(LogType::LOG_INFO,"从二次定位平台，移动到CNC机台门前方");
             state = moveto_CNC_front_door_from_locationplane();
             if(state == false) return;

             //检测放料允许信号
             log_message(LogType::LOG_INFO,"检测放料允许信号");
             state = waitfor_putting_raw_signal(-1);

             //检测光电信号，是否开门
             log_message(LogType::LOG_INFO,"检测光电信号，是否开门");
             state = waitfor_opening_CNC_door(300000);
             if(state == false){
                 exception_when_opening_door();
                 stop_work_flow();
                 QMessageBox::information(this, "提示\t", "CNC机台分钟未开门\t");
                 return;
             }


             //移动到CNC机台内部放料位
             log_message(LogType::LOG_INFO,"移动到CNC机台内部放料位");
             state = moveto_CNC_baseplane_putting_pose();
             if(state == false) return;

             //吹料
             log_message(LogType::LOG_INFO,"吹料");
             state = mainthread_robot_arm_->set_DO(6,0);
             state = mainthread_robot_arm_->set_DO(3,1);

             //从CNC机台内部，移动回初始化待机点
             log_message(LogType::LOG_INFO,"从CNC机台内部，移动回初始化待机点");
             state = moveto_initialpose_from_CNC();
             if(state == false) return;

             has_workpeice_in_localization = false;

             endTime = QTime::currentTime();

             //QTime duration = endTime - startTime;

        }else{
             //qDebug() << "waitfor_taking_workpiece_signal";
        }

        */

    }

void MainWindow::on_spinBox_joint_speed_valueChanged(int arg1)
{
    JointSpeed = arg1;
}

void MainWindow::on_spinBox_joint_acceleration_valueChanged(int arg1)
{
    JointACC = arg1;
}

void MainWindow::on_spinBox_joint_deceleration_valueChanged(int arg1)
{
    JointDEC = arg1;
}

void MainWindow::on_pushButton_modbus_write_68_clicked()
{
    int value = ui->spinBox_modbus_coil_value_68->value();

    NDC_Client_.write_AGV_data_68(value);
}

void MainWindow::on_button_load_all_station_data_clicked()
{

    QProgressDialog progressDialog(this);
    progressDialog.setCancelButtonText(tr("取消"));
    progressDialog.setRange(0, 10);
    progressDialog.setWindowTitle(tr("加载机台参数"));

    progressDialog.show();

    //连接机械臂
    progressDialog.setValue(1);
    progressDialog.setLabelText(tr("连接机械臂"));
    sleep_ms(500);
    qApp->processEvents();
    bool state = connect_to_robotarm();

    if(state == false){
        QMessageBox::warning(this, "提示\t", "连接机械臂失败\t");
        return;
    }

    //加载机械臂通用点位
    progressDialog.setValue(2);
    progressDialog.setLabelText(tr("加载机械臂通用点位"));
    sleep_ms(500);
    qApp->processEvents();
    state = load_common_pos();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "加载机械臂通用点位失败\t");
        return;
    }


    //加载机台点位
    progressDialog.setValue(3);
    progressDialog.setLabelText(tr("加载机台点位"));
    sleep_ms(500);
    qApp->processEvents();
    state = load_station_pos();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "加载机台点位失败\t");
        return;
    }

    //打开光源
    progressDialog.setValue(4);
    progressDialog.setLabelText(tr("连接光源控制器"));
    sleep_ms(500);
    qApp->processEvents();
    ui->comboBox_Port->setCurrentIndex(1);
    state = openSerial();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "连接光源控制器失败\t");
        return;
    }

    //打开机械臂相机
    progressDialog.setValue(5);
    progressDialog.setLabelText(tr("打开机械臂相机"));
    qApp->processEvents();
    state = openCamera_arm();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "打开机械臂相机失败\t");
        return;
    }


    //加载九点标定数据
    progressDialog.setValue(6);
    progressDialog.setLabelText(tr("加载九点标定数据"));
    sleep_ms(500);
    qApp->processEvents();
    state = load_calibration();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "加载九点标定数据失败\t");
        return;
    }

    //连接modbus
    progressDialog.setValue(7);
    progressDialog.setLabelText(tr("连接modbus"));
    sleep_ms(500);
    qApp->processEvents();
    state = openModbus();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "连接modbus失败\t");
        return;
    }


    //打开读DM码相机
    progressDialog.setValue(8);
    progressDialog.setLabelText(tr("打开读DM码相机"));
    qApp->processEvents();
    state = openCamera_dm();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "打开读DM码相机失败\t");
        return;
    }

    //加载模板大小
    progressDialog.setValue(9);
    progressDialog.setLabelText(tr("加载模板大小"));
    sleep_ms(500);
    qApp->processEvents();
    state = load_station_template_info();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "加载模板大小失败\t");
        return;
    }

    //读取机台二维码
    progressDialog.setValue(10);
    progressDialog.setLabelText(tr("加载机台二维码信息"));
    sleep_ms(500);
    qApp->processEvents();
    state = load_station_qr_info();
    if(state == false){
        QMessageBox::warning(this, "提示\t", "加载机台二维码信息失败\t");
        return;
    }

    QMessageBox::information(this,"软件初始化","加载所有数据完成，初始化完成!");


}

//取生料异常
bool MainWindow::exception_when_taking_raw_at_transfer(){
    //移动到取生料点上方
    bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
                                                       MoveType::LINE,
                                                       LineSpeed,LineACC,LineDEC,
                                                       BlockMotion::LOCKED);

    if(state == false) return false;


    //移动到转移平台前方
    state = mainthread_robot_arm_->move_to_common_pose("转移平台前方",
                                                       MoveType::LINE,
                                                       LineSpeed,LineACC,LineDEC,
                                                       BlockMotion::LOCKED);

    if(state == false) return false;

    //移动到转移平台前方上方
    state = mainthread_robot_arm_->move_to_common_pose("转移平台前方上方",
                                                       MoveType::LINE,
                                                       LineSpeed,LineACC,LineDEC,
                                                       BlockMotion::LOCKED);

    if(state == false) return false;


    //移动回初始化待机点
    state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                       MoveType::JOINT,
                                                       JointSpeed,JointACC,JointDEC,
                                                       BlockMotion::LOCKED);

    return state;
}

//在二次定位平台取生料异常
bool MainWindow::exception_when_taking_raw_at_localization(){

    bool state = moveto_initialpose_from_locationplane();
    return state;
}

//视觉拍照异常
bool MainWindow::exception_when_processing_vision(){

    QString type = calibration_.get_station_type(current_station_id);


    bool state = false;

    if(type == "左机台"){
        //移动左边拍照待机点
        state = mainthread_robot_arm_->move_to_common_pose("左边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,LineACC,LineDEC,
                                                            BlockMotion::LOCKED);

      //  state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
      //                                                     MoveType::JOINT,
      //                                                      JointSpeed,JointACC,JointDEC,
      //                                                      BlockMotion::LOCKED);

        if(state == false){
            stop_work_flow();
            return false;
        }

    }
    else if(type == "右机台"){
        //移动右边拍照待机点
        bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,LineACC,LineDEC,
                                                            BlockMotion::LOCKED);

        if(state == false){
            stop_work_flow();
            return false;
        }
       // state = mainthread_robot_arm_->move_to_common_pose("水槽上方待机点",
       //                                                     MoveType::JOINT,
       //                                                     JointSpeed,JointACC,JointDEC,
       //                                                     BlockMotion::LOCKED);


        state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,JointACC,JointDEC,
                                                            BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

    }


     state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                       MoveType::LINE,
                                                       LineSpeed,LineACC,LineDEC,
                                                       BlockMotion::LOCKED);

     if(state == false){
         stop_work_flow();
         return false;
     }

    return state;

}
//取熟料异常
bool MainWindow::exception_when_taking_workpiece(){

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }
    //移动到丢废料点
    state = mainthread_robot_arm_->move_to_pose(station_correct_discard_pose,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }
    //打开夹爪,丢弃废料
    state = open_jaw();

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
    }
    else if(type == "右机台"){
        state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

        state = mainthread_robot_arm_->move_to_common_pose("水槽上方待机点",
                                                         MoveType::LINE,
                                                         LineSpeed,LineACC,LineDEC,
                                                         BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }
    }


    state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                     MoveType::LINE,
                                                     LineSpeed,LineACC,LineDEC,
                                                     BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }

    //send_finishing_taking_signal();

    return state;
}

//机台门打开异常
bool MainWindow::exception_when_opening_door(){


    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        bool state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }


         state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
         return state;

    }
    else if(type == "右机台"){
        bool state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }
         state = mainthread_robot_arm_->move_to_common_pose("水槽上方待机点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
         state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
         return state;
    }

}

bool MainWindow::exception_when_no_workpiece_in_CNC()
{
    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }
    //移动到丢废料点
    state = mainthread_robot_arm_->move_to_pose(station_correct_discard_pose,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }
    //打开夹爪,丢弃废料
    state = open_jaw();

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

        //NDC_Client_.write_ADV_data_64(1);
        state = send_finishing_taking_signal();

        if(state == false){
            stop_work_flow();
            return false;
        }

         state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false){
             stop_work_flow();
             return false;
         }
    }
    else if(type == "右机台"){
        state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }

        //NDC_Client_.write_ADV_data_64(1);
        state = send_finishing_taking_signal();

        if(state == false){
            stop_work_flow();
            return false;
        }

        state = mainthread_robot_arm_->move_to_common_pose("水槽上方待机点",
                                                         MoveType::LINE,
                                                         LineSpeed,LineACC,LineDEC,
                                                         BlockMotion::LOCKED);
        if(state == false){
            stop_work_flow();
            return false;
        }
    }


    state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                     MoveType::LINE,
                                                     LineSpeed,LineACC,LineDEC,
                                                     BlockMotion::LOCKED);
    if(state == false){
        stop_work_flow();
        return false;
    }

    NDC_Client_.write_ADV_data_66(1);
    sleep_ms(500);
    NDC_Client_.write_ADV_data_67(1);

    return state;
}

bool MainWindow::stop_work_flow(){

    //流程停止
    timer_work_flow_->stop();
    //发送红灯信号
    NDC_Client_.write_AGV_data_69(1);

    ui->startFlowButton->setEnabled(true);

    current_step = 34;

    QMessageBox::information(this, "提示\t", "流程已停止\t");

    return true;
}

//连接机械臂
bool MainWindow::connect_to_robotarm(){
    QString host = ui->line_edit_ads_robotarm->text();
    QString port = ui->line_edit_port_robotarm->text();
    bool state = mainthread_robot_arm_->connectToHost(host.toStdString(),port.toInt());

    if(state == true){
        emit sig_start_monitor();
    }

    return state;
}
//加载机械臂公共点
bool MainWindow::load_common_pos(){
    QString DirPath = QCoreApplication::applicationDirPath() + "\\";
    QString DirName = "RobotArmPose\\Common";

    QString RobotPoseData_Dir = DirPath + DirName +"\\robot_common_pose.json";
    //打开文件
    QFile file(RobotPoseData_Dir);
    if(!file.exists())
    {
        QMessageBox::critical(this, "错误\t", "找不到该文件!\t");
        return false;
    }
    file.open(QIODevice::ReadOnly);
    QByteArray data=file.readAll();
    //file.close();
    //使用json文件对象加载字符串
    QJsonDocument doc=QJsonDocument::fromJson(data);
    //判断是否对象
    if(doc.isObject())
    {
        //把json文档转换为json对象
        QJsonObject obj=doc.object();

        for(const QString& key : obj.keys()){

            QJsonValue jvalue = obj[key];
            double pose[6];
            if(jvalue.isArray()){
                QJsonArray array = jvalue.toArray();
                int nSize = array.size();

                for(int i=0; i < nSize; ++i){
                    QJsonValue value = array.at(i);
                    if(value.isDouble()){
                        pose[i] = value.toDouble();
                    }
                }
            }

            RobotPoseJoint joint_pose ={
                pose[0],
                pose[1],
                pose[2],
                pose[3],
                pose[4],
                pose[5]
            };

            mainthread_robot_arm_->add_common_pose(key,joint_pose);

            //write to table
            int countIndex = ui->tableWidget_robotarm_common_pos->rowCount();

            ui->tableWidget_robotarm_common_pos->setRowCount(countIndex + 1);
            ui->tableWidget_robotarm_common_pos->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

            ui->tableWidget_robotarm_common_pos->setItem(countIndex,0,new
                                                  QTableWidgetItem(key));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,1,
                                                  new QTableWidgetItem(QString::number(pose[0],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,2,
                                                  new QTableWidgetItem(QString::number(pose[1],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,3,
                                                  new QTableWidgetItem(QString::number(pose[2],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,4,
                                                  new QTableWidgetItem(QString::number(pose[3],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,5,
                                                  new QTableWidgetItem(QString::number(pose[4],'f',6)));
            ui->tableWidget_robotarm_common_pos->setItem(countIndex,6,
                                                  new QTableWidgetItem(QString::number(pose[5],'f',6)));
        }

    }

    return true;
}
//加载机械臂机台点
bool MainWindow::load_station_pos(){
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "RobotArmPose/Station/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            //QString message = "机台 " + it + " 无点位数据\t";
            //QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                QJsonValue jvalue = obj[key];
                double pose[6];
                if(jvalue.isArray()){
                    QJsonArray array = jvalue.toArray();
                    int nSize = array.size();

                    for(int i=0; i < nSize; ++i){
                        QJsonValue value = array.at(i);
                        if(value.isDouble()){
                            pose[i] = value.toDouble();
                        }
                    }
                }

                RobotPoseXYZ joint_pose ={
                    pose[0],
                    pose[1],
                    pose[2],
                    pose[3],
                    pose[4],
                    pose[5]
                };

                mainthread_robot_arm_->add_station_pose(it,key,joint_pose);

            }
        }

    }

    if(station_ids.size() != 0){
       ui->comboBox_station_name->setCurrentIndex(0);
       on_comboBox_station_name_currentIndexChanged(0);
    }

    return true;

    //QMessageBox::information(this, "提示\t", "加载机台数据完成\t");
}
//打开光源串口
bool MainWindow::openSerial(){
    SerialParam param;
    param.port = ui->comboBox_Port->currentText();
    param.baud = ui->comboBox_Baud->currentText();
    param.dataBit = ui->comboBox_Bit->currentText();
    param.stopBits = ui->comboBox_Stop->currentText();
    param.parity = ui->comboBox_Parity->currentText();

    if(light_->setSerialParam(param)){
        return true;
        //QMessageBox::information(this,"Light","Open Light Success!");
    }
    else{
        return false;
        //QMessageBox::information(this,"Light","Open Light Fail!");
    }
}
//打开机械臂相机
bool MainWindow::openCamera_arm(){
    bool state = camera_->turnOnCamera(CameraName::RobotArm);

    if(state == true){
        //QMessageBox::information(this,"Open Camera ","Open Camera Success!");
        ui->checkBox_cameraState->setChecked(true);
    }
    else{
        //QMessageBox::information(this,"Open Camera ","Open Camera Fail!");
        ui->checkBox_cameraState->setChecked(false);
    }

    return state;
}
//加载标定数据
bool MainWindow::load_calibration(){
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "Calibration/TransformMatrix/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            //QString message = "机台 " + it + " 无转换矩阵数据\t";
            //QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                if(key == "wrap_matrix"){

                    QJsonValue jvalue = obj[key];
                    double matrix[6];
                    if(jvalue.isArray()){
                        QJsonArray array = jvalue.toArray();
                        int nSize = array.size();

                        for(int i=0; i < nSize; ++i){
                            QJsonValue value = array.at(i);
                            if(value.isDouble()){
                                matrix[i] = value.toDouble();
                            }
                        }
                    }

                    cv::Mat wrap_mat(2,3,CV_32FC1);
                    wrap_mat.at<float>(0,0) = matrix[0];
                    wrap_mat.at<float>(0,1) = matrix[1];
                    wrap_mat.at<float>(0,2) = matrix[2];
                    wrap_mat.at<float>(1,0) = matrix[3];
                    wrap_mat.at<float>(1,1) = matrix[4];
                    wrap_mat.at<float>(1,2) = matrix[5];

                    calibration_.add_station_transform_matrix(it, wrap_mat);

                }

                if(key == "reference_pose"){

                    QJsonValue jvalue = obj[key];
                    double pose[6];
                    if(jvalue.isArray()){
                        QJsonArray array = jvalue.toArray();
                        int nSize = array.size();

                        for(int i=0; i < nSize; ++i){
                            QJsonValue value = array.at(i);
                            if(value.isDouble()){
                                pose[i] = value.toDouble();
                            }
                        }
                    }

                    RobotPoseXYZ reference_pose = {
                        pose[0], pose[1], pose[2],
                        pose[3], pose[4], pose[5]
                    };

                    calibration_.add_station_reference_pose(it, reference_pose);
                }

                if(key == "reference_angle"){
                     QJsonValue jvalue = obj[key];

                     if(jvalue.isDouble()){
                         double angle = jvalue.toDouble();
                         calibration_.add_station_reference_angle(it, angle);
                     }
                }

                if(key == "type"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isString()){
                        QString type = jvalue.toString();
                        calibration_.add_station_type(it, type);
                    }
                }

                if(key == "exposure_time"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isDouble()){
                        int value = jvalue.toDouble();
                        calibration_.add_station_exposure_time(it, value);
                    }
                }

            }
        }

    }

    if(station_ids.size() != 0){
        ui->comboBox_station_name_matrix->setCurrentIndex(0);
        on_comboBox_station_name_matrix_currentIndexChanged(0);
    }

    //QMessageBox::information(this, "提示\t", "加载机台标定数据完成\t");
    return true;
}
//连接modbus服务器
bool MainWindow::openModbus(){
    const QUrl url = QUrl::fromUserInput(ui->portEdit->text());

    std::string host = url.host().toStdString();
    int port = url.port();

    bool state = NDC_Client_.connectToHost(host, port);

    if(state == true){
        //QMessageBox::information(this,"提示\t","成功连接到Modbus\t");
    }
    else{
        //QMessageBox::information(this,"提示\t","连接到Modbus失败\t");
    }

    return state;
}
//打开读Dm码相机
bool MainWindow::openCamera_dm(){
    bool state = camera_->turnOnCamera(CameraName::DataMatrix);

    if(state == true){
        //QMessageBox::information(this,"Open Camera ","Open Camera Success!");
        ui->checkBox_cameraState_dm->setChecked(true);
    }
    else{
        //QMessageBox::information(this,"Open Camera ","Open Camera Fail!");
        ui->checkBox_cameraState_dm->setChecked(false);
    }

    return state;
}
//加载模板长宽信息
bool MainWindow::load_station_template_info(){
    //读取所有机台信号
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "StationsModel/";

        QString RobotPoseData_Dir = DirPath +
                                    DirName +
                                    it + "//" +
                                    it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            //QString message = "机台 " + it + " 无模板数据\t";
            //QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                QJsonValue jvalue = obj[key];
                int image_info[2];
                if(jvalue.isArray()){
                    QJsonArray array = jvalue.toArray();
                    int nSize = array.size();

                    for(int i=0; i < nSize; ++i){
                        QJsonValue value = array.at(i);
                        if(value.isDouble()){
                            image_info[i] = value.toDouble();
                        }
                    }
                }

                if(image_info[0] != 0 && image_info[1] != 0){
                    ModelInfo model_info = {image_info[0], image_info[1]};
                    image_processor_.add_station_model_info(it,model_info);
                }

            }
        }

    }

    //显示所有机台数据
    auto station_model_info = image_processor_.get_station_model_info();

    /*
    for(auto it : station_model_info){

        //write to table
        int countIndex = ui->tableWidget_stations_model_info->rowCount();

        ui->tableWidget_stations_model_info->setRowCount(countIndex + 1);
        ui->tableWidget_stations_model_info->setVerticalHeaderItem(countIndex,new QTableWidgetItem(QString::number(countIndex+1)));

        ui->tableWidget_stations_model_info->setItem(countIndex,0,new
                                              QTableWidgetItem(it.first));
        ui->tableWidget_stations_model_info->setItem(countIndex,1,
                                              new QTableWidgetItem(QString::number(it.second.width)));
        ui->tableWidget_stations_model_info->setItem(countIndex,2,
                                              new QTableWidgetItem(QString::number(it.second.height)));

    }
    */

    //QMessageBox::information(this, "提示\t", "加载机台标定数据完成\t");
    return true;
}

//加载机台二维码信息
bool MainWindow::load_station_qr_info(){

    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "StationQR/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无二维码数据\t";
            QMessageBox::critical(this, "错误\t", message);
            return false;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                if(key == "QR"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isString()){
                        QString qr_info = jvalue.toString();

                        if(station_id_mapper.find(it) == station_id_mapper.end()){
                            station_id_mapper.emplace(it,qr_info);
                        }
                        else{
                            station_id_mapper.at(it) = qr_info;
                        }
                    }
                }

            }
        }

    }

    if(station_ids.size() != 0){
        ui->comboBox_station_name_qr->setCurrentIndex(0);
        on_comboBox_station_name_qr_currentIndexChanged(0);
    }

    return true;

}

void MainWindow::on_button_stop_reading_dm_clicked()
{
    stop_reading_dm = true;
}

void MainWindow::on_radioButton_speed_medium_clicked()
{
    ui->spinBox_speed->setValue(200); //200
    ui->spinBox_acceleration->setValue(50); //100
    ui->spinBox_deceleration->setValue(5);

    ui->spinBox_joint_speed->setValue(50); //100
    ui->spinBox_joint_acceleration->setValue(25); //50
    ui->spinBox_joint_deceleration->setValue(5);
}

void MainWindow::on_radioButton_speed_low_clicked()
{
    ui->spinBox_speed->setValue(30);
    ui->spinBox_acceleration->setValue(15);
    ui->spinBox_deceleration->setValue(5);

    ui->spinBox_joint_speed->setValue(5);
    ui->spinBox_joint_acceleration->setValue(5);
    ui->spinBox_joint_deceleration->setValue(5);
}

void MainWindow::on_radioButton_speed_fast_clicked()
{
    ui->spinBox_speed->setValue(500);
    ui->spinBox_acceleration->setValue(50);
    ui->spinBox_deceleration->setValue(5);

    ui->spinBox_joint_speed->setValue(100);
    ui->spinBox_joint_acceleration->setValue(20);
    ui->spinBox_joint_deceleration->setValue(20);
}

void MainWindow::log_message(LogType type, QString message){

    int rows = ui->tableWidget_log->rowCount();

    if(rows == 20){
        ui->tableWidget_log->setRowCount(0);
    }

    rows = ui->tableWidget_log->rowCount();
    //qDebug() << "rpws " << rows;
    ui->tableWidget_log->setRowCount(rows + 1);
    //ui->tableWidget_log->setItem(rows,0,new QTableWidgetItem(QString::number(rows)));

    QTableWidgetItem* item = nullptr;
    switch (type) {
    case LogType::LOG_INFO:
        item = new QTableWidgetItem("info");
        item->setBackgroundColor(QColor(0,255,0));
        ui->tableWidget_log->setItem(rows,0,item);
        qDebug() << message;
        break;
    case LogType::LOG_WARNING:

        item = new QTableWidgetItem("warning");
        item->setBackgroundColor(QColor(255,255,0));
        ui->tableWidget_log->setItem(rows,0,item);
        qWarning() << message;
        break;
    case LogType::LOG_ERROR:
        item = new QTableWidgetItem("error");
        item->setBackgroundColor(QColor(255,0,0));
        ui->tableWidget_log->setItem(rows,0,item);
        qCritical() << message;
        break;
    default:
        break;
    }

    ui->tableWidget_log->setItem(rows,1,new QTableWidgetItem(message));

}

void MainWindow::on_button_move_by_path_clicked()
{
    //return;
    //添加路点

    RobotPoseXYZ pose_1;
    mainthread_robot_arm_->getRobotStationPose("0","测试点",pose_1);

    RobotPoseXYZ pose_2;
    mainthread_robot_arm_->getRobotStationPose("0","相机拍照点",pose_2);



    mainthread_robot_arm_->add_common_waypoint("test3",MoveType::JOINT,30,10);
    mainthread_robot_arm_->add_common_waypoint("test2",MoveType::JOINT,30,10);
    mainthread_robot_arm_->add_common_waypoint("test1",MoveType::JOINT,30,10);
    mainthread_robot_arm_->add_xyz_waypoint(pose_1,MoveType::JOINT,30,10);
    mainthread_robot_arm_->add_xyz_waypoint(pose_2,MoveType::LINE,30,10);

    //执行轨迹运动
    mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    //清除路点
    //mainthread_robot_arm_->clear_waypoints();
}

void MainWindow::on_button_stop_test_clicked()
{
    //设置伺服状态为false
    mainthread_robot_arm_->set_servo_status(false);
}


/****************************************************************************************
 *
 *                                 run_all_work_flow
 *
 *
 * **************************************************************************************/

void MainWindow::run_all_work_flow(){
    /*************************
          //先从转移平台取生料
    *************************/

    //如果接到取生料请求
    if(waitfor_taking_raw_signal(1000)
       && has_workpeice_in_localization == false)
    {

        //检测机械臂DI，进行防呆
        int data;
        bool di_state = mainthread_robot_arm_->get_DI(12,data);

        if(di_state == true && data == 1){
             log_message(LogType::LOG_INFO, "get robotarm di-12 = 1");
            //do nothing
        }
        else{
            return;
        }

        log_message(LogType::LOG_INFO, "-------------------------------------------------");
        log_message(LogType::LOG_INFO, "station start working.");
        totalTime.start();

        stepTime.start();

        //从初始化待机点到转移平台前方
       log_message(LogType::LOG_INFO,"step-1: 从初始化待机点到转移平台前方");

       bool state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                   MoveType::JOINT,
                                                   JointSpeed,
                                                   10);

       if(state == false || work_flag == false){
           stop_work_flow();
           return;
       }

         //移动到转移平台取生料位
         log_message(LogType::LOG_INFO,"step-2: 移动到转移平台取生料位");

         //移动到取生料点上方
         state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return;
        }

        //移动到取生料点
        state = mainthread_robot_arm_->add_common_waypoint("取生料点",
                                                   MoveType::LINE,
                                                   LineSpeed,
                                                   10);

       if(state == false || work_flag == false){
           stop_work_flow();
           return;
       }

       //发送轨迹路径2.0
       state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

       if(state == false || work_flag == false){
           stop_work_flow();
           return ;
       }


         //吸料
          log_message(LogType::LOG_INFO,"step-3: 吸料");
         state = mainthread_robot_arm_->set_DO(6,1);

         //等待1秒
         sleep_ms(1000);

         //从转移平台移动到二次定位平台放料点
         log_message(LogType::LOG_INFO,"step-4: 从转移平台移动到二次定位平台放料点");
          state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
                                                    MoveType::LINE,
                                                    LineSpeed,LineACC,LineDEC,
                                                    BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return;
         }

         //真空检测
         int suck_ok = 0;
         bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

          //吸生料异常处理
         if(receive == false || suck_ok == 0){

             //关闭吸真空
             mainthread_robot_arm_->set_DO(6,0);
             //异常，将料再吹一下
             chow_workpiece();
             //
             exception_when_taking_raw_at_transfer();
             stop_work_flow();
             QMessageBox::information(this, "提示\t", "转移平台吸生料异常\t");
             return;
         }

         //去二次定位平台放料
         state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

          if(state == false || work_flag == false){
              stop_work_flow();
              return;
          }

         state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return;
         }

         state = mainthread_robot_arm_->add_common_waypoint("二次平台放料点",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return;
         }

         //发送轨迹运动2.0
         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return;
         }

         //给吹料信号
         log_message(LogType::LOG_INFO,"step-5: 给吹料信号");
         state = mainthread_robot_arm_->set_DO(6,0);
         state = mainthread_robot_arm_->set_DO(3,1);

         state = mainthread_robot_arm_->add_common_waypoint("二次平台放料点上方",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return;
         }

         //移动到吹DM码位置
         log_message(LogType::LOG_INFO,"step-6: 移动到吹DM码位置");
         state = mainthread_robot_arm_->add_common_waypoint("吹DM点",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         //关闭吹料
         log_message(LogType::LOG_INFO,"step-7: 关闭吹料");
         state = mainthread_robot_arm_->set_DO(3,0);

         //吹DM码
         log_message(LogType::LOG_INFO,"step-7: 吹DM码");
         state = chow_DMcode();

         //从二次定位平台移动回待机点
         log_message(LogType::LOG_INFO,"step-8: 从二次定位平台移动回待机点");

         state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                     MoveType::LINE,
                                                     LineSpeed,
                                                     10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }


         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }


         ui->doubleSpinBox_ct_1->setValue(stepTime.elapsed()/1000.0);

         //循环，直到识别DM码
          log_message(LogType::LOG_INFO,"step-9: 循环，直到识别DM码");
          dm_message = "";
          int decode_time = 0;
         stepTime.start();
         while(1){

             if(decode_time >8){
                 NDC_Client_.write_AGV_data_69(1);
                 QMessageBox::information(this, "提示\t", "读取DM码失败，请更换二次定位平台生料\t");
                 decode_time = 0;
                 NDC_Client_.write_AGV_data_69(0);
                 continue;
             }

             //设置曝光时间
             int exposure_time = 5000 + decode_time*5000;//15000 + decode_time*5000;
             camera_->set_exposure_time(CameraName::DataMatrix,exposure_time);

             state = decode_DMcode();

             if(state == true){
                 break;
             }

             if(stop_reading_dm == true){
                 break;
                 stop_reading_dm = false;
             }

             QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
             decode_time++;
             //break;
         }

         ui->doubleSpinBox_ct_2->setValue(stepTime.elapsed()/1000.0);

         has_workpeice_in_localization = true;

    }

    /*************************
        //先进行从CNC取熟料
    *************************/

    //如果接到取CNC加工料的机台信号

    if(has_workpeice_in_localization == true
        && waitfor_taking_workpiece_signal(1000)){

        log_message(LogType::LOG_INFO,"step-10: 接收到机台 " + current_station_id + " 取CNC加工料的机台信号");


        //qDebug() << "开始上传DM码";
        log_message(LogType::LOG_INFO,"开始上传DM码");
        if(!dm_message.empty()){
            bool state = send_dm_message(QString::fromStdString(dm_message), 300000);
            if(state == true){
               log_message(LogType::LOG_INFO,"上传DM码信息成功");
               //qDebug() << "上传DM码信息成功";

            }
            else{
                log_message(LogType::LOG_WARNING,"上传DM码超时5分钟，请检查网络是否断开连接");
                QMessageBox::information(this, "提示\t", "上传DM码超时5分钟，请检查网络是否断开连接\t");
                stop_work_flow();
                return;
            }
        }
        else{
            log_message(LogType::LOG_WARNING,"dm_message为空");
            QMessageBox::information(this, "提示\t", "dm_message为空\t");
            stop_work_flow();
            return;
        }



        stepTime.start();
        //移动到相机拍照位
        log_message(LogType::LOG_INFO,"step-11: 移动到相机拍照位");

        QString type = calibration_.get_station_type(current_station_id);

        if(type == "左机台"){

            bool state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                        MoveType::JOINT,
                                                        JointSpeed,
                                                        10);
            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

          state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点二",
                                                      MoveType::LINE,
                                                      LineSpeed,
                                                      10);

          if(state == false || work_flag == false){
              stop_work_flow();
              return ;
          }

            //移动右边拍照待机点
            state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                                "相机拍照点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }



            state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }
        }
        else if(type == "右机台")
        {
            //移动右边拍照待机点
              bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
                                                                MoveType::JOINT,
                                                                JointSpeed,JointACC,JointDEC,
                                                                BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

            //移动右边拍照待机点
            state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                                "相机拍照点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

            state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

        }

        //视觉拍照前，先关闭空压机
        NDC_Client_.write_AGV_data_68(1);

         //视觉定位
         log_message(LogType::LOG_INFO,"step-12: 视觉定位");
         //stepTime.start();
         bool state = process_image_and_correct_pose();
         //qDebug() << "视觉定位时间： " <<stepTime.elapsed()/1000.0 << " s";
         ui->doubleSpinBox_ct_3->setValue(stepTime.elapsed()/1000.0);
         //视觉拍照异常处理流程
         if(state == false){
             exception_when_processing_vision();
             //抓取熟料视觉拍照，不需要发送异常
             NDC_Client_.write_ADV_data_64(2);
             sleep_ms(15000);

             log_message(LogType::LOG_WARNING,
                         current_station_id+ "机台视觉拍照异常");
             return;
         }

         //拍照完，可以打开空压机
         stepTime.start();
         log_message(LogType::LOG_INFO,"step-13: 拍照完，可以打开空压机");
         NDC_Client_.write_AGV_data_68(0);

         //从视觉拍照点，移动到CNC机台门前方
         log_message(LogType::LOG_INFO,"step-14: 从视觉拍照点，移动到CNC机台门前方");
         type = calibration_.get_station_type(current_station_id);

         if(type == "左机台"){

             bool state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点",
                                                         MoveType::LINE,
                                                         LineSpeed,
                                                         10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

              state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                                  MoveType::LINE,
                                                                  LineSpeed,
                                                                  10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                                  MoveType::LINE,
                                                                  LineSpeed,
                                                                  10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

         }
         else if(type == "右机台"){
             bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                                 MoveType::JOINT,
                                                                 JointSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

              state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                                  MoveType::LINE,
                                                                  LineSpeed,
                                                                  10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }
         }

         //检测光电信号，大门是否打开
         log_message(LogType::LOG_INFO,"step-15: 检测光电信号，大门是否打开");
         state = waitfor_opening_CNC_door(300000);
         if(state == false){
             exception_when_opening_door();
             stop_work_flow();
             log_message(LogType::LOG_ERROR,
                         current_station_id+ "机台5分钟未开门");
             QMessageBox::information(this, "提示\t", "CNC机台5分钟未开门\t");
             return;
         }

         //移动到CNC内部取料位
         log_message(LogType::LOG_INFO,"step-16: 移动到CNC内部取料位");

         //张开小夹爪
         open_jaw();

         //移动到CNC机台OK料位上方
         RobotPoseXYZ station_correct_pose_above = station_correct_pose;

         station_correct_pose_above.z += 20;//mm

//         mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
//                                                 MoveType::LINE,
//                                                 LineSpeed,
//                                                 2);

         state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                     MoveType::LINE,
                                                     LineSpeed,LineACC,LineDEC,
                                                     BlockMotion::LOCKED);


         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         //吹料1秒
         state = chow_only(1000);

         if(state == false){
             stop_work_flow();
             return ;
         }

         RobotPoseXYZ station_correct_pose_take_finished = station_correct_pose;

             station_correct_pose_take_finished.z -= 5;//mm

         //移动到CNC机台OK料位
//         mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_take_finished,
//                                                 MoveType::LINE,
//                                                 30,
//                                                 1);

         state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
                                                     MoveType::LINE,
                                                     30,10,10,
                                                     BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

//         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

//         if(state == false){
//             stop_work_flow();
//             return ;
//         }

         //关闭小夹爪
         close_jaw();

         //吸料
         log_message(LogType::LOG_INFO,"step-17: 吸料");
         state = mainthread_robot_arm_->set_DO(6,1);
         sleep_ms(1000);

         if(state == false) return;

         //真空检测
         log_message(LogType::LOG_INFO,"step-18: 真空检测");

         //检测CNC机台是否有熟料
         bool has_workpeice_in_CNC = false;
         int suck_ok = 0;
         bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

         if(receive == false){
             QMessageBox::information(this, "提示\t", "读取机械臂DI失败\t");
             stop_work_flow();
             return;
         }

         //使用接近开关检测物料
         int check_ok = 0;
         bool receive_2 = mainthread_robot_arm_->get_DI(7, check_ok);

         if(receive_2 == false){
             QMessageBox::information(this, "提示\t", "读取机械臂DI失败\t");
             stop_work_flow();
             return;
         }

         if(suck_ok == 0){
             has_workpeice_in_CNC = false;
             //关闭吸真空
             mainthread_robot_arm_->set_DO(6,0);

             //吹料
             chow_workpiece();
             //return ;

         }
         else{
             has_workpeice_in_CNC = true;
         }

         //吸到料，执行吸料后正常流程
         log_message(LogType::LOG_INFO,"step-19: 吸到料，执行吸料后正常流程");

         if(has_workpeice_in_CNC == true){

             //移动到水槽
             log_message(LogType::LOG_INFO,"step-20: 移动到水槽");
             //移动到CNC机台OK料位上方
             RobotPoseXYZ station_correct_pose_above = station_correct_pose;

             station_correct_pose_above.z += 20;//mm

             bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                         MoveType::LINE,
                                                         80,40,40,
                                                         BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //再做一次真空检测
             int suck_ok = 0;
             bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);
             if(receive == false ){
                 while (1) {
                     Sleep(1000);
                     receive = mainthread_robot_arm_->get_DI(10, suck_ok);
                     if(receive == true ){break;}
                 }

             }
             if( suck_ok == 0){
                 bool state = chow_workpiece();
                 exception_when_taking_workpiece();

                 stop_work_flow();
                 QMessageBox::information(this, "提示\t", "CNC取熟料真空异常\t");
                 return ;
             }


             //移动到丢废料点
             state = mainthread_robot_arm_->move_to_pose(station_correct_discard_pose,
                                                         MoveType::LINE,
                                                         LineSpeed,LineACC,LineDEC,
                                                         BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //打开夹爪,丢弃废料
             state = open_jaw();

             QString type = calibration_.get_station_type(current_station_id);

             if(type == "左机台"){

                 state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                                   MoveType::LINE,
                                                                   LineSpeed,LineACC,LineDEC,
                                                                   BlockMotion::LOCKED);

                 if(state == false || work_flag == false){
                     stop_work_flow();
                     return ;
                 }

                 //发送取料完成信号
                 log_message(LogType::LOG_INFO,"发送取料完成信号");
                 state = send_finishing_taking_signal();

                 if(state == false){
                     stop_work_flow();
                     return;
                 }


                 //打开抽水泵(
                  log_message(LogType::LOG_INFO,"step-21: 打开抽水泵");
                 start_washing_workpiece();

                  state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                      MoveType::JOINT,
                                                                      JointSpeed,
                                                                      10);

                  if(state == false || work_flag == false){
                      stop_work_flow();
                      return ;
                  }

                  state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                                                      MoveType::JOINT,
                                                                      JointSpeed,
                                                                      10);

                  if(state == false || work_flag == false){
                      stop_work_flow();
                      return ;
                  }


             }
             else if(type == "右机台"){
                 state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                                   MoveType::LINE,
                                                                   LineSpeed,LineACC,LineDEC,
                                                                   BlockMotion::LOCKED);

                 if(state == false || work_flag == false){
                     stop_work_flow();
                     return ;
                 }

                 //发送取料完成信号
                 log_message(LogType::LOG_INFO,"发送取料完成信号");
                 state = send_finishing_taking_signal();

                 if(state == false){
                     stop_work_flow();
                     return;
                 }

             }

             //后面和CT关系不大

             //打开抽水泵(
              log_message(LogType::LOG_INFO,"step-21: 打开抽水泵");
             start_washing_workpiece();

             state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
                                                                 MoveType::JOINT,
                                                                 JointSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->add_common_waypoint("洗料池",
                                                                 MoveType::LINE,
                                                                 LineSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }



             //上下移动清洗
             log_message(LogType::LOG_INFO,"step-22: 上下移动清洗");
             RobotPoseJoint pose_j;
             mainthread_robot_arm_->getRobotCommonPose("洗料池", pose_j);

             RobotPoseXYZ pose_xyz;
             mainthread_robot_arm_->getFK(pose_j,pose_xyz);

             for(int i=0; i<3; i++){

                 bool state = mainthread_robot_arm_->add_xyz_waypoint(pose_xyz,
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

                 if(state == false || work_flag == false){
                     stop_work_flow();
                     return ;
                 }

                 RobotPoseXYZ temp_pose = pose_xyz;
                 temp_pose.z -= 100;
                 //移动到洗料池上方
                 state = mainthread_robot_arm_->add_xyz_waypoint(temp_pose,
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

                 if(state == false || work_flag == false){
                     stop_work_flow();
                     return ;
                 }
             }

             ui->doubleSpinBox_ct_4->setValue(stepTime.elapsed()/1000.0);

             //从水槽，移动到转移平台前方
             stepTime.start();
             log_message(LogType::LOG_INFO,"step-23: 从水槽，移动到转移平台前方");
             //移动到
             state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
                                                                 MoveType::LINE,
                                                                 LineSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //移动到
             state = mainthread_robot_arm_->add_common_waypoint("洗料池上方沥水点",
                                                                 MoveType::LINE,
                                                                 LineSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //关闭抽水泵
             log_message(LogType::LOG_INFO,"step-24: 关闭抽水泵");
             state = close_washing_workpiece();

               //沥水5秒钟
               sleep_ms(5000);

               //等待转移平台放料允许信号
               log_message(LogType::LOG_INFO,"step-25: 等待转移平台放料允许信号");
               state = waitfor_transfer_allowing_signal(-1);

               //然后去转移平台放熟料
               state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                                          MoveType::JOINT,
                                                          JointSpeed,
                                                          10);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                          MoveType::JOINT,
                                                          JointSpeed,
                                                          10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }


             //移动到转移平台放料位
             log_message(LogType::LOG_INFO,"step-26: 移动到转移平台放料位");
              state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                         MoveType::LINE,
                                                         LineSpeed,
                                                         10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //移动到取生料点
            state = mainthread_robot_arm_->add_common_waypoint("放熟料点",
                                                           MoveType::LINE,
                                                           15,
                                                           10);
            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

            state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

             //吹料
             log_message(LogType::LOG_INFO,"step-27: 吹料");
             state = chow_workpiece();

             //从转移平台，移动到初始化待机点
             log_message(LogType::LOG_INFO,"step-28: 从转移平台，移动到初始化待机点");
             //移动到取生料点上方

              state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                         MoveType::LINE,
                                                         LineSpeed,
                                                         10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //移动回初始化待机点
             state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                             MoveType::JOINT,
                                                             JointSpeed,
                                                             10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //发送转移平台允许移动信号
             log_message(LogType::LOG_INFO,"step-29: 发送转移平台允许移动信号");
             state = send_transfer_allowing_signal();

             ui->doubleSpinBox_ct_5->setValue(stepTime.elapsed()/1000.0);
         }
         else{

             //吸真空检测异常，有2中情况，没有物料，有铝屑没有吸起来
             state = mainthread_robot_arm_->set_DO(6,0);

             //如果接近开关检测有物料
             if(check_ok == 1){

                 //返回待机点
                 exception_when_taking_workpiece();

                 QMessageBox::information(this, "提示\t", "机台有熟料，未吸起来\t");
                 //stop_work_flow();

                 //return;
             }
             else{
                 exception_when_no_workpiece_in_CNC();
             }
         }

        /*************************************************************************
              //接着进行放生料到CNC
        **************************************************************************/

         //移动到相机拍照位
          log_message(LogType::LOG_INFO,"step-30: 移动到相机拍照位");
          stepTime.start();

           type = calibration_.get_station_type(current_station_id);


          if(type == "左机台"){

              bool state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,
                                                          10);


              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

            state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点二",
                                                        MoveType::LINE,
                                                        LineSpeed,
                                                        10);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }


              //移动拍照待机点
            state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                                "相机拍照点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

            state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

            if(state == false || work_flag == false){
                stop_work_flow();
                return ;
            }

          }
          else if(type == "右机台")
          {
              //移动右边拍照待机点
                bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,
                                                            10);

                if(state == false || work_flag == false){
                    stop_work_flow();
                    return ;
                }

              //移动右边拍照待机点
              state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                                  "相机拍照点",
                                                                  MoveType::LINE,
                                                                  LineSpeed,
                                                                  10);


              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

          }

          //视觉拍照前，先关闭空压机
          NDC_Client_.write_AGV_data_68(1);

         //视觉定位
          log_message(LogType::LOG_INFO,"step-31: 视觉定位");

         state = process_image_and_correct_pose();

         //视觉拍照异常处理流程
         if(state == false){
             exception_when_processing_vision();

             stop_work_flow();
             log_message(LogType::LOG_ERROR,
                         current_station_id + "机台视觉拍照异常");
             QMessageBox::information(this, "提示\t", "视觉拍照异常\t");
             return;
         }

         //打开空压机
         log_message(LogType::LOG_INFO,"step-32: 打开空压机");
         NDC_Client_.write_AGV_data_68(0);
         //从相机拍照位，移动到二次定位平台
         log_message(LogType::LOG_INFO,"step-33: 从相机拍照位，移动到二次定位平台");
          type = calibration_.get_station_type(current_station_id);

         if(type == "左机台"){
             //移动左边拍照待机点    
             bool state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点",
                                                                 MoveType::LINE,
                                                                 LineSpeed,
                                                                 10);


             if(state == false || work_flag == false){
                 stop_work_flow();
                 return;
             }

         }
         else if(type == "右机台"){

             bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                                 MoveType::LINE,
                                                                 LineSpeed,
                                                                 10);
             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

              state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                 MoveType::JOINT,
                                                                 JointSpeed,
                                                                 10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

              //return state;
         }


          state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("吸二次定位平台生料上方点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }


         state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点",
                                                                MoveType::LINE,
                                                                15,
                                                                10);

         if(state == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         //吸料
         log_message(LogType::LOG_INFO,"step-34: 吸料");
         state = suck_workpiece();
         //吸生料异常处理
         if(state == false){
             //关闭吸真空
             mainthread_robot_arm_->set_DO(6,0);
             //再吹一下，使物料落在二次定位平台
             chow_workpiece();
             //
             exception_when_taking_raw_at_localization();
             stop_work_flow();
             QMessageBox::information(this, "提示\t", "二次定位平台取生料异常\t");
             return;
         }

         ui->doubleSpinBox_ct_6->setValue(stepTime.elapsed()/1000.0);

         stepTime.start();

         //从二次定位平台，移动到CNC机台门前方
         log_message(LogType::LOG_INFO,"step-35: 从二次定位平台，移动到CNC机台门前方");
          type = calibration_.get_station_type(current_station_id);

         if(type == "左机台"){

             bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                             MoveType::LINE,
                                                                             LineSpeed,
                                                                             10);
             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }


             state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                                    MoveType::LINE,
                                                                    LineSpeed,
                                                                    10);
             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

               state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                     MoveType::LINE,
                                                                     LineSpeed,
                                                                     10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                                    MoveType::JOINT,
                                                                    JointSpeed,
                                                                    10);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                                    MoveType::LINE,
                                                                    LineSpeed,
                                                                    10);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

         }
         else if(type == "右机台"){

             bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                             MoveType::LINE,
                                                                             LineSpeed,
                                                                             10);
             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                                   MoveType::LINE,
                                                                   LineSpeed,
                                                                   10);
             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

              state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                    MoveType::LINE,
                                                                    LineSpeed,
                                                                    10);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                                    MoveType::JOINT,
                                                                    JointSpeed,
                                                                    10);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

              state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }
         }

         ui->doubleSpinBox_ct_7->setValue(stepTime.elapsed()/1000.0);

         stepTime.start();


         //检测放料允许信号
         //stepTime.start();
         log_message(LogType::LOG_INFO,"step-36: 检测放料允许信号");
         state = waitfor_putting_raw_signal(-1);
         //qDebug()<< "等待放料允许信号时间：" << stepTime.elapsed()/1000.0 << "s";

         //检测光电信号，是否开门
         log_message(LogType::LOG_INFO,"step-37: 检测光电信号，是否开门");
         //stepTime.start();
         state = waitfor_opening_CNC_door(300000);
         if(state == false){
             exception_when_opening_door();
             stop_work_flow();
             QMessageBox::information(this, "提示\t", "CNC机台分钟未开门\t");
             return;
         }
         //qDebug()<< "等待开门光电检测时间：" << stepTime.elapsed()/1000.0 << "s";
         ui->doubleSpinBox_ct_8->setValue(stepTime.elapsed()/1000.0);

         stepTime.start();

         //移动到CNC机台内部放料位
         log_message(LogType::LOG_INFO,"step-38: 移动到CNC机台内部放料位");

         //移动到CNC机台OK料位上方
         station_correct_pose_above = station_correct_pose;

         station_correct_pose_above.z += 20;//mm

          state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                     MoveType::LINE,
                                                     LineSpeed,LineACC,LineDEC,
                                                     BlockMotion::LOCKED);

//          state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
//                                                              MoveType::LINE,
//                                                              LineSpeed,
//                                                              2);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

          station_correct_pose_take_finished = station_correct_pose;

         //移动到CNC机台OK料位
         state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
                                                     MoveType::LINE,
                                                     50,15,15,
                                                     BlockMotion::LOCKED);

//          state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_take_finished,
//                                                              MoveType::LINE,
//                                                              LineSpeed,
//                                                              10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }


         //吹料
         log_message(LogType::LOG_INFO,"step-39: 吹料");
         state = mainthread_robot_arm_->set_DO(6,0);
         state = mainthread_robot_arm_->set_DO(3,1);

         //从CNC机台内部，移动回初始化待机点
         log_message(LogType::LOG_INFO,"step-40: 从CNC机台内部，移动回初始化待机点");
         //移动到CNC机台OK料位上方
         station_correct_pose_above = station_correct_pose;

         station_correct_pose_above.z += 20;//mm

          state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
                                                              MoveType::LINE,
                                                              120,
                                                              10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

          type = calibration_.get_station_type(current_station_id);

         if(type == "左机台"){

             state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);


             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //关闭吹料
             state = mainthread_robot_arm_->set_DO(3,0);
             //发送放料完成信号
             state = send_finishing_putting_signal();

             if(state == false){
                 stop_work_flow();
                 return;
             }

             ui->doubleSpinBox_ct_total->setValue(totalTime.elapsed()/1000.0);

             state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                               MoveType::JOINT,
                                                               JointSpeed,JointACC,JointDEC,
                                                               BlockMotion::LOCKED);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }

         }
         else if(type == "右机台"){

             state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

             if(state == false || work_flag == false){
                 stop_work_flow();
                 return ;
             }

             //关闭吹料
             state = mainthread_robot_arm_->set_DO(3,0);
             //发送放料完成信号
             state = send_finishing_putting_signal();

             if(state == false){
                 stop_work_flow();
                 return;
             }

             ui->doubleSpinBox_ct_total->setValue(totalTime.elapsed()/1000.0);

              state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                               MoveType::JOINT,
                                                               JointSpeed,JointACC,JointDEC,
                                                               BlockMotion::LOCKED);
              if(state == false || work_flag == false){
                  stop_work_flow();
                  return ;
              }
         }

         ui->doubleSpinBox_ct_9->setValue(stepTime.elapsed()/1000.0);

         has_workpeice_in_localization = false;

    }else{
         //qDebug() << "waitfor_taking_workpiece_signal";
    }
}

/****************************************************************************************
 *
 *                                 //运行状态机模式
 *
 *
 * **************************************************************************************/

void MainWindow::run_statemachine_flow(){

    current_step = 0;

    while(1){

        switch (current_step) {
        case 0://检查是否接到取生料请求
            run_step_0();
            break;
        case 1://机械臂移动到转移平台取生料点
            run_step_1();
            break;
        case 2://吸料
            run_step_2();
            break;
        case 3://从转移平台移动到二次定位平台放料点
            run_step_3();
            break;
        case 4://给吹料信号
            run_step_4();
            break;
        case 5://移动到吹DM码位置
            run_step_5();
            break;
        case 6://吹DM码
            run_step_6();
            break;
        case 7://从二次定位平台移动回待机点
            run_step_7();
            break;
        case 8://循环，直到识别DM码
            run_step_8();
            break;
        case 9://如果接到取CNC加工料的机台信号
            run_step_9();
            break;
        case 10://开始上传DM码
            run_step_10();
            break;
        case 11://移动到相机拍照位
            run_step_11();
            break;
        case 12://视觉定位
            run_step_12();
            break;
        case 13://从视觉拍照点，移动到CNC机台门前方
            run_step_13();
            break;
        case 14://检测光电信号，大门是否打开
            run_step_14();
            break;
        case 15://移动到CNC内部取料位
            run_step_15();
            break;
        case 16://吸料及真空检测、接近开关检测
            run_step_16();
            break;
        case 17://丢弃废料后，移动到CNC门前点
            run_step_17();
            break;
        case 18://打开抽水泵,并上下清洗
            run_step_18();
            break;
        case 19://等待转移平台放料允许信号
            run_step_19();
            break;
        case 20://然后去转移平台放熟料
            run_step_20();
            break;
        case 21://吹料
            run_step_21();
            break;
        case 22://从转移平台，移动到初始化待机点
            run_step_22();
            break;
        case 23://吸真空检测异常，有2中情况，没有物料，有铝屑没有吸起来
            run_step_23();
            break;
        case 24://移动到相机拍照位
            run_step_24();
            break;
        case 25://视觉定位
            run_step_25();
            break;
        case 26: //从相机拍照位，移动到二次定位平台
            run_step_26();
            break;
        case 27: //二次定位平台吸生料
            run_step_27();
            break;
        case 28://从二次定位平台，移动到CNC机台门前方
            run_step_28();
            break;
        case 29://检测放料允许信号
            run_step_29();
            break;
        case 30://检测光电信号，是否开门
            run_step_30();
            break;
        case 31://移动到CNC机台内部放料位
            run_step_31();
            break;
        case 32://吹料
            run_step_32();
            break;
        case 33://从CNC机台内部，移动回初始化待机点
            run_step_33();
            break;
        case 34://手动结束流程
            return;
        default:
            break;
        }

        sleep_ms(20);
    }


    
}


void MainWindow::on_pushButton_T3_S8_2_clicked()
{
    bool state = moveto_CNC_baseplane_putting_pose();

    if(state == false){
        set_test_state("Test3",9,false);
    }

    set_test_state("Test3",9,true);
}

void MainWindow::on_pushButton_get_station_state_clicked()
{
    QString station_id = ui->comboBox_station_name_qr->currentText();

    if(station_id_mapper.find(station_id) == station_id_mapper.end()){
        QMessageBox::information(this, "提示\t", "CNC机台未绑定二维码\t");
        return;
    }

    QString MachineSn = station_id_mapper.at(station_id);

    int state = -1;
    bool post_state = http_client_.operate_get_status(MachineSn,state);

    if(post_state == false){
        QMessageBox::information(this, "提示\t", "网络请求失败，请检查网络\t");
    }

    ui->label_station_state->setNum(state);

}

void MainWindow::on_pushButton_bind_station_qr_clicked()
{
    QString DirPath = QCoreApplication::applicationDirPath();
    QString DirBaseName = "StationQR";

    mkdir_ifneed(DirPath +"/" + DirBaseName);

    QString station_name = ui->comboBox_station_name_qr->currentText();
    if(station_name.isEmpty()){
        QMessageBox::information(this, "提示\t", "机台名不能为空\t");
        return;
    }

    QFile stationQR(DirPath + "/"
                        + DirBaseName + "/"
                        + station_name + ".json");

    //QIODevice::Append
    if(!stationQR.open(QIODevice::WriteOnly | QIODevice::Text ))
    {
        return;
    }

    QJsonDocument jsonDoc;
    QJsonObject jsonObj;

    QString station_qr_info = ui->portEdit_station_qr->text();
    jsonObj.insert("QR",station_qr_info);

    if(station_qr_info.isEmpty()){
        QString message = "机台 " + station_name + " 二维码不能为空\t";
        QMessageBox::information(this, "提示\t", message);
        return;
    }

    if(station_id_mapper.find(station_name) == station_id_mapper.end()){
        station_id_mapper.emplace(station_name,station_qr_info);
    }
    else{
        station_id_mapper.at(station_name) = station_qr_info;
    }

    //保存到本地
    jsonDoc.setObject(jsonObj);
    stationQR.write(jsonDoc.toJson());
    stationQR.close();
    //QString id = ui->comboBox_station_name_qr->currentText();
    QString message = "机台 " + station_name + " 绑定机台二维码完成\t";
    QMessageBox::information(this, "提示\t", message);
}

void MainWindow::on_pushButton_load_station_qr_clicked()
{
    read_stationid_fromfile();

    for(auto it: station_ids){

        QString DirPath = QCoreApplication::applicationDirPath() + "/";
        QString DirName = "StationQR/";

        QString RobotPoseData_Dir = DirPath + DirName + it + ".json";
        //打开文件
        QFile file(RobotPoseData_Dir);
        if(!file.exists())
        {
            QString message = "机台 " + it + " 无二维码数据\t";
            QMessageBox::critical(this, "错误\t", message);
            continue;
        }

        file.open(QIODevice::ReadOnly);
        QByteArray data=file.readAll();
        //file.close();
        //使用json文件对象加载字符串
        QJsonDocument doc=QJsonDocument::fromJson(data);
        //判断是否对象
        if(doc.isObject())
        {
            //把json文档转换为json对象
            QJsonObject obj=doc.object();

            for(const QString& key : obj.keys()){

                if(key == "QR"){

                    QJsonValue jvalue = obj[key];
                    if(jvalue.isString()){
                        QString qr_info = jvalue.toString();

                        if(station_id_mapper.find(it) == station_id_mapper.end()){
                            station_id_mapper.emplace(it,qr_info);
                        }
                        else{
                            station_id_mapper.at(it) = qr_info;
                        }
                    }
                }

            }
        }

    }

    if(station_ids.size() != 0){
        ui->comboBox_station_name_qr->setCurrentIndex(0);
        on_comboBox_station_name_qr_currentIndexChanged(0);
    }

    QMessageBox::information(this, "提示\t", "加载机台二维码数据完成\t");
}

void MainWindow::on_comboBox_station_name_qr_currentIndexChanged(int index)
{
    QString name = ui->comboBox_station_name_qr->currentText();

    if(station_id_mapper.find(name) != station_id_mapper.end()){
        QString qr_info = station_id_mapper.at(name);

        ui->portEdit_station_qr->setText(qr_info);
    }
    else{
        ui->portEdit_station_qr->setText("");
    }

}

void MainWindow::on_pushButton_open_station_door_clicked()
{
    QString station_id = ui->comboBox_station_name_qr->currentText();

    if(station_id_mapper.find(station_id) == station_id_mapper.end()){
        QMessageBox::information(this, "提示\t", "CNC机台未绑定二维码\t");
        return;
    }

    QString MachineSn = station_id_mapper.at(station_id);

    int state = -1;
    bool post_state = http_client_.operate_open_door(MachineSn,state);

    if(post_state == false){
        QMessageBox::information(this, "提示\t", "网络请求失败，请检查网络\t");
    }
}

void MainWindow::on_pushButton_close_station_door_clicked()
{
    QString station_id = ui->comboBox_station_name_qr->currentText();

    if(station_id_mapper.find(station_id) == station_id_mapper.end()){
        QMessageBox::information(this, "提示\t", "CNC机台未绑定二维码\t");
        return;
    }

    QString MachineSn = station_id_mapper.at(station_id);

    int state = -1;
    bool post_state = http_client_.operate_close_door(MachineSn,state);

    if(post_state == false){
        QMessageBox::information(this, "提示\t", "网络请求失败，请检查网络\t");
    }
}

void MainWindow::on_pushButton_wash_CNC_station_clicked()
{
    QString station_id = ui->comboBox_station_name_qr->currentText();

    if(station_id_mapper.find(station_id) == station_id_mapper.end()){
        QMessageBox::information(this, "提示\t", "CNC机台未绑定二维码\t");
        return;
    }

    QString MachineSn = station_id_mapper.at(station_id);

    int state = -1;
    bool post_state = http_client_.operate_blow(MachineSn,state);

    if(post_state == false){
        QMessageBox::information(this, "提示\t", "网络请求失败，请检查网络\t");
    }
}

void MainWindow::on_button_refine_station_pos_clicked()
{
    int current_index = ui->tableWidget_robotarm_station_pos->currentRow();

    if(current_index == -1){
        QMessageBox::information(this,"提示\t","选中要修正行\t");
        return;
    }

    RobotPoseXYZ station_correct_pose_before = station_correct_pose;

    RobotPoseXYZ current_pose = mainthread_robot_arm_->getRobotPoseXYZ();


    QString station_id = ui->comboBox_station_name->currentText();
    current_index = ui->tableWidget_robotarm_station_pos->currentRow();
    QString pose_name = ui->tableWidget_robotarm_station_pos->item(current_index,0)->text();

    if(pose_name == "相机拍照点"){
        QMessageBox::information(this,"提示\t","相机拍照点不可修正\t");
        return;
    }

    RobotPoseXYZ target_pose;
    mainthread_robot_arm_->getRobotStationPose(station_id,
                                               pose_name,
                                               target_pose);
    RobotPoseXYZ redine_pose;

    bool state = calibration_.refine_station_pose(station_correct_pose_before,
                                                  current_pose,
                                                  target_pose,
                                                  redine_pose);

    if(state == true){

        ui->tableWidget_robotarm_station_pos->setItem(current_index,1,new QTableWidgetItem(QString::number(redine_pose.x , 'f', 6 )));
        ui->tableWidget_robotarm_station_pos->setItem(current_index,2,new QTableWidgetItem(QString::number(redine_pose.y , 'f', 6 )));
        ui->tableWidget_robotarm_station_pos->setItem(current_index,3,new QTableWidgetItem(QString::number(redine_pose.z , 'f', 6 )));
        ui->tableWidget_robotarm_station_pos->setItem(current_index,4,new QTableWidgetItem(QString::number(redine_pose.rx , 'f', 6 )));
        ui->tableWidget_robotarm_station_pos->setItem(current_index,5,new QTableWidgetItem(QString::number(redine_pose.ry , 'f', 6 )));
        ui->tableWidget_robotarm_station_pos->setItem(current_index,6,new QTableWidgetItem(QString::number(redine_pose.rz , 'f', 6 )));

        QMessageBox::information(this, "提示\t", "修正点位，保存后生效\t");
    }
}


//运行流程
void MainWindow::run_step_0(){
    //检查是否接到取生料请求
    if(waitfor_taking_raw_signal(1000)
       && has_workpeice_in_localization == false)
    {
        //检测机械臂DI，进行防呆
        int data;
        bool di_state = mainthread_robot_arm_->get_DI(12,data);
    
        if(di_state == true && data == 1){
             log_message(LogType::LOG_INFO, "get robotarm di-12 = 1");
            //do nothing
             current_step = 1;
        }
    }
    
}

//机械臂移动到转移平台取生料点
void MainWindow::run_step_1(){

    log_message(LogType::LOG_INFO, "-------------------------------------------------");
    log_message(LogType::LOG_INFO, "station start working.");
    //totalTime.start();

    //stepTime.start();

    //从初始化待机点到转移平台前方
   log_message(LogType::LOG_INFO,"step-1: 从初始化待机点到转移平台前方");

   bool state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                               MoveType::JOINT,
                                               JointSpeed,
                                               10);

   if(state == false || work_flag == false){
       stop_work_flow();
       return;
   }

     //移动到转移平台取生料位
     log_message(LogType::LOG_INFO,"step-2: 移动到转移平台取生料位");

     //移动到取生料点上方
     state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }

    //移动到取生料点
    state = mainthread_robot_arm_->add_common_waypoint("取生料点",
                                               MoveType::LINE,
                                               LineSpeed,
                                               10);

   if(state == false || work_flag == false){
       stop_work_flow();
       return;
   }

   //发送轨迹路径2.0
   state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

   if(state == false || work_flag == false){
       stop_work_flow();
       return ;
   }

   current_step = 2;
}

//吸料
void MainWindow::run_step_2(){
    //吸料
    log_message(LogType::LOG_INFO,"step-3: 吸料");
    mainthread_robot_arm_->set_DO(6,1);

    //等待1秒
    sleep_ms(1000);

    current_step = 3;
}

//从转移平台移动到二次定位平台放料点
void MainWindow::run_step_3(){
    //从转移平台移动到二次定位平台放料点
    log_message(LogType::LOG_INFO,"step-4: 从转移平台移动到二次定位平台放料点");
    /*
    bool state = mainthread_robot_arm_->move_to_common_pose("取生料点上方",
                                               MoveType::LINE,
                                               LineSpeed,LineACC,LineDEC,
                                               BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }
    */

    bool state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

     if(state == false || work_flag == false){
         stop_work_flow();
         return;
     }

     //发送轨迹运动2.0
     state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

     if(state == false || work_flag == false){
         stop_work_flow();
         return;
     }

    //真空检测
    int suck_ok = 0;
    bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

     //吸生料异常处理
    if(receive == false || suck_ok == 0){

        //关闭吸真空
        mainthread_robot_arm_->set_DO(6,0);
        //异常，将料再吹一下
        chow_workpiece();
        //
        exception_when_taking_raw_at_transfer();
        stop_work_flow();
        QMessageBox::information(this, "提示\t", "转移平台吸生料异常\t");
        return;
    }

    //去二次定位平台放料
    state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

     if(state == false || work_flag == false){
         stop_work_flow();
         return;
     }

    state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }

    state = mainthread_robot_arm_->add_common_waypoint("二次平台放料点",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }

    //发送轨迹运动2.0
    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }

    current_step = 4;
}

//给吹料信号
void MainWindow::run_step_4(){
    //给吹料信号
    log_message(LogType::LOG_INFO,"step-5: 给吹料信号");
    mainthread_robot_arm_->set_DO(6,0);
    mainthread_robot_arm_->set_DO(3,1);

    current_step = 5;
}

//移动到吹DM码位置
void MainWindow::run_step_5(){
    //移动到吹DM码位置
    log_message(LogType::LOG_INFO,"step-6: 移动到吹DM码位置");
    bool state = mainthread_robot_arm_->add_common_waypoint("二次平台放料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return;
    }

    state = mainthread_robot_arm_->add_common_waypoint("吹DM点",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    current_step = 6;
}

//
void MainWindow::run_step_6(){
    //关闭吹料
    log_message(LogType::LOG_INFO,"step-7: 关闭吹料");
    mainthread_robot_arm_->set_DO(3,0);

    //吹DM码
    log_message(LogType::LOG_INFO,"step-7: 吹DM码");
    chow_DMcode();

    current_step = 7;
}

//从二次定位平台移动回待机点
void MainWindow::run_step_7(){
    //从二次定位平台移动回待机点
    log_message(LogType::LOG_INFO,"step-8: 从二次定位平台移动回待机点");

    bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }


    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    current_step = 8;
    //ui->doubleSpinBox_ct_1->setValue(stepTime.elapsed()/1000.0);
}

void MainWindow::run_step_8(){
    //循环，直到识别DM码
     log_message(LogType::LOG_INFO,"step-9: 循环，直到识别DM码");
     dm_message = "";
     int decode_time = 0;
    //stepTime.start();
    while(1){

        if(decode_time >8){
            NDC_Client_.write_AGV_data_69(1);
            QMessageBox::information(this, "提示\t", "读取DM码失败，请更换二次定位平台生料\t");
            decode_time = 0;
            NDC_Client_.write_AGV_data_69(0);
            continue;
        }

        //设置曝光时间
        int exposure_time = 5000 + decode_time*5000;//15000 + decode_time*5000;
        camera_->set_exposure_time(CameraName::DataMatrix,exposure_time);

        bool state = decode_DMcode();

        if(state == true){
            break;
        }

        if(stop_reading_dm == true){
            break;
            stop_reading_dm = false;
        }

        QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
        decode_time++;
        //break;
    }

    //ui->doubleSpinBox_ct_2->setValue(stepTime.elapsed()/1000.0);

    has_workpeice_in_localization = true;

    current_step = 9;
}

void MainWindow::run_step_9(){

    //如果接到取CNC加工料的机台信号
    if(has_workpeice_in_localization == true
        && waitfor_taking_workpiece_signal(1000)){

       log_message(LogType::LOG_INFO,"step-10: 接收到机台 " + current_station_id + " 取CNC加工料的机台信号");

        current_step = 10;
    }else{
         //qDebug() << "waitfor_taking_workpiece_signal";
    }

}

void MainWindow::run_step_10(){

    //qDebug() << "开始上传DM码";
    log_message(LogType::LOG_INFO,"开始上传DM码");
    if(!dm_message.empty()){
        bool state = send_dm_message(QString::fromStdString(dm_message), 300000);
        if(state == true){
           log_message(LogType::LOG_INFO,"上传DM码信息成功");
           //qDebug() << "上传DM码信息成功";

        }
        else{
            log_message(LogType::LOG_WARNING,"上传DM码超时5分钟，请检查网络是否断开连接");
            QMessageBox::information(this, "提示\t", "上传DM码超时5分钟，请检查网络是否断开连接\t");
            stop_work_flow();
            return;
        }
    }
    else{
        log_message(LogType::LOG_WARNING,"dm_message为空");
        QMessageBox::information(this, "提示\t", "dm_message为空\t");
        stop_work_flow();
        return;
    }

    current_step = 11;
}

void MainWindow::run_step_11(){

    //stepTime.start();
    //移动到相机拍照位
    log_message(LogType::LOG_INFO,"step-11: 移动到相机拍照位");

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                    MoveType::JOINT,
                                                    JointSpeed,
                                                    10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

      state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点二",
                                                  MoveType::LINE,
                                                  LineSpeed,
                                                  10);

      if(state == false || work_flag == false){
          stop_work_flow();
          return ;
      }

        //移动右边拍照待机点
        state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                            "相机拍照点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }



        state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }
    }
    else if(type == "右机台")
    {
        //移动右边拍照待机点
          bool state = mainthread_robot_arm_->move_to_common_pose("右边拍照待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,JointACC,JointDEC,
                                                            BlockMotion::LOCKED);

          if(state == false || work_flag == false){
              stop_work_flow();
              return ;
          }

        //移动右边拍照待机点
        state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                            "相机拍照点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

    }

    current_step = 12;

}

void MainWindow::run_step_12(){
    //视觉拍照前，先关闭空压机
    NDC_Client_.write_AGV_data_68(1);

     //视觉定位
     log_message(LogType::LOG_INFO,"step-12: 视觉定位");
     //stepTime.start();
     bool state = process_image_and_correct_pose();
     //qDebug() << "视觉定位时间： " <<stepTime.elapsed()/1000.0 << " s";
     //ui->doubleSpinBox_ct_3->setValue(stepTime.elapsed()/1000.0);
     //视觉拍照异常处理流程
     if(state == false){
         exception_when_processing_vision();
         //抓取熟料视觉拍照，不需要发送异常
         NDC_Client_.write_ADV_data_64(2);
         sleep_ms(15000);

         log_message(LogType::LOG_WARNING,
                     current_station_id+ "机台视觉拍照异常");

         current_step = 9;
         return;
     }

     //拍照完，可以打开空压机
     //stepTime.start();
     log_message(LogType::LOG_INFO,"step-13: 拍照完，可以打开空压机");
     NDC_Client_.write_AGV_data_68(0);

     current_step = 13;
}

void MainWindow::run_step_13(){
    //从视觉拍照点，移动到CNC机台门前方
    log_message(LogType::LOG_INFO,"step-14: 从视觉拍照点，移动到CNC机台门前方");
    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        bool state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点",
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

    }
    else if(type == "右机台"){
        bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,
                                                            10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

         state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }
    }

    current_step = 14;
}

void MainWindow::run_step_14(){

    //检测光电信号，大门是否打开
    log_message(LogType::LOG_INFO,"step-15: 检测光电信号，大门是否打开");
    bool state = waitfor_opening_CNC_door(300000);
    if(state == false){
        exception_when_opening_door();
        stop_work_flow();
        log_message(LogType::LOG_ERROR,
                    current_station_id+ "机台5分钟未开门");
        QMessageBox::information(this, "提示\t", "CNC机台5分钟未开门\t");
        return;
    }

    current_step = 15;
}

void MainWindow::run_step_15(){
    //移动到CNC内部取料位
    log_message(LogType::LOG_INFO,"step-16: 移动到CNC内部取料位");

    //张开小夹爪
    open_jaw();

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

//         mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
//                                                 MoveType::LINE,
//                                                 LineSpeed,
//                                                 2);

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);


    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //吹料1秒
    state = chow_only(1000);

    if(state == false){
        stop_work_flow();
        return ;
    }

    RobotPoseXYZ station_correct_pose_take_finished = station_correct_pose;

        station_correct_pose_take_finished.z -= 5;//mm

    //移动到CNC机台OK料位
//         mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_take_finished,
//                                                 MoveType::LINE,
//                                                 30,
//                                                 1);

    state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
                                                MoveType::LINE,
                                                30,10,10,
                                                BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

//         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

//         if(state == false){
//             stop_work_flow();
//             return ;
//         }

    //关闭小夹爪
    close_jaw();

    current_step = 16;
}

//吸料及真空检测、接近开关检测
void MainWindow::run_step_16(){
    //吸料
    log_message(LogType::LOG_INFO,"step-17: 吸料");
    bool state = mainthread_robot_arm_->set_DO(6,1);
    sleep_ms(1000);

    if(state == false) return;

    //真空检测
    log_message(LogType::LOG_INFO,"step-18: 真空检测");

    //检测CNC机台是否有熟料
    bool has_workpeice_in_CNC = false;
    int suck_ok = 0;
    bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);

    if(receive == false){
        QMessageBox::information(this, "提示\t", "读取机械臂DI失败\t");
        stop_work_flow();
        return;
    }

    //使用接近开关检测物料
    bool receive_2 = mainthread_robot_arm_->get_DI(7, check_by_BERO);

    if(receive_2 == false){
        QMessageBox::information(this, "提示\t", "读取机械臂DI失败\t");
        stop_work_flow();
        return;
    }

    if(suck_ok == 0){
        has_workpeice_in_CNC = false;
        //关闭吸真空
        mainthread_robot_arm_->set_DO(6,0);

        //吹料
        chow_workpiece();
        //return ;
        current_step = 23;
    }
    else{
        has_workpeice_in_CNC = true;

        current_step = 17;
    }
}

void MainWindow::run_step_17(){
    //丢弃废料后，移动到CNC门前点
    log_message(LogType::LOG_INFO,"step-20: 丢弃废料后，移动到CNC门前点");
    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                80,40,40,
                                                BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //再做一次真空检测
    int suck_ok = 0;
    bool receive = mainthread_robot_arm_->get_DI(10, suck_ok);
    if(receive == false ){
        while (1) {
            Sleep(1000);
            receive = mainthread_robot_arm_->get_DI(10, suck_ok);
            if(receive == true ){break;}
        }

    }
    if( suck_ok == 0){
        bool state = chow_workpiece();
        exception_when_taking_workpiece();

        stop_work_flow();
        QMessageBox::information(this, "提示\t", "CNC取熟料真空异常\t");
        return ;
    }


    //移动到丢废料点
    state = mainthread_robot_arm_->move_to_pose(station_correct_discard_pose,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //打开夹爪,丢弃废料
    state = open_jaw();

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->move_to_common_pose("左边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        //发送取料完成信号
        log_message(LogType::LOG_INFO,"发送取料完成信号");
        state = send_finishing_taking_signal();

        if(state == false){
            stop_work_flow();
            return;
        }

        //打开抽水泵(
         log_message(LogType::LOG_INFO,"step-21: 打开抽水泵");
        start_washing_workpiece();

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                             MoveType::JOINT,
                                                             JointSpeed,
                                                             10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                                             MoveType::JOINT,
                                                             JointSpeed,
                                                             10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }
    }
    else if(type == "右机台"){
        state = mainthread_robot_arm_->move_to_common_pose("右边门前点",
                                                          MoveType::LINE,
                                                          LineSpeed,LineACC,LineDEC,
                                                          BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        //发送取料完成信号
        log_message(LogType::LOG_INFO,"发送取料完成信号");
        state = send_finishing_taking_signal();

        if(state == false){
            stop_work_flow();
            return;
        }

    }

    current_step = 18;
}

//打开抽水泵,并上下清洗
void MainWindow::run_step_18(){
    //打开抽水泵(
     log_message(LogType::LOG_INFO,"step-21: 打开抽水泵");
    start_washing_workpiece();

    bool state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
                                                        MoveType::JOINT,
                                                        JointSpeed,
                                                        10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->add_common_waypoint("洗料池",
                                                        MoveType::LINE,
                                                        LineSpeed,
                                                        10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }



    //上下移动清洗
    log_message(LogType::LOG_INFO,"step-22: 上下移动清洗");
    RobotPoseJoint pose_j;
    mainthread_robot_arm_->getRobotCommonPose("洗料池", pose_j);

    RobotPoseXYZ pose_xyz;
    mainthread_robot_arm_->getFK(pose_j,pose_xyz);

    for(int i=0; i<3; i++){

        bool state = mainthread_robot_arm_->add_xyz_waypoint(pose_xyz,
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        RobotPoseXYZ temp_pose = pose_xyz;
        temp_pose.z -= 100;
        //移动到洗料池上方
        state = mainthread_robot_arm_->add_xyz_waypoint(temp_pose,
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }
    }

    //ui->doubleSpinBox_ct_4->setValue(stepTime.elapsed()/1000.0);

    //从水槽，移动到转移平台前方
    //stepTime.start();
    log_message(LogType::LOG_INFO,"step-23: 从水槽，移动到转移平台前方");
    //移动到
    state = mainthread_robot_arm_->add_common_waypoint("洗料池上方",
                                                        MoveType::LINE,
                                                        LineSpeed,
                                                        10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //移动到
    state = mainthread_robot_arm_->add_common_waypoint("洗料池上方沥水点",
                                                        MoveType::LINE,
                                                        LineSpeed,
                                                        10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //关闭抽水泵
    log_message(LogType::LOG_INFO,"step-24: 关闭抽水泵");
    state = close_washing_workpiece();

      //沥水5秒钟
      sleep_ms(5000);

     current_step = 19;
}

void MainWindow::run_step_19(){
    //等待转移平台放料允许信号
    log_message(LogType::LOG_INFO,"step-25: 等待转移平台放料允许信号");
    waitfor_transfer_allowing_signal(-1);

     current_step = 20;
}


void MainWindow::run_step_20(){
    //然后去转移平台放熟料
    bool state = mainthread_robot_arm_->add_common_waypoint("左边机台去水槽上方",
                                               MoveType::JOINT,
                                               JointSpeed,
                                               10);
   if(state == false || work_flag == false){
       stop_work_flow();
       return ;
   }

   state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                               MoveType::JOINT,
                                               JointSpeed,
                                               10);

   if(state == false || work_flag == false){
       stop_work_flow();
       return ;
   }


  //移动到转移平台放料位
  log_message(LogType::LOG_INFO,"step-26: 移动到转移平台放料位");
   state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                              MoveType::LINE,
                                              LineSpeed,
                                              10);

  if(state == false || work_flag == false){
      stop_work_flow();
      return ;
  }

  //移动到取生料点
 state = mainthread_robot_arm_->add_common_waypoint("放熟料点",
                                                MoveType::LINE,
                                                15,
                                                10);
 if(state == false || work_flag == false){
     stop_work_flow();
     return ;
 }

 state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

 if(state == false || work_flag == false){
     stop_work_flow();
     return ;
 }

      current_step = 21;
}

void MainWindow::run_step_21(){
    //吹料
    log_message(LogType::LOG_INFO,"step-27: 吹料");
    chow_workpiece();

         current_step = 22;
}

void MainWindow::run_step_22(){
    //从转移平台，移动到初始化待机点
    log_message(LogType::LOG_INFO,"step-28: 从转移平台，移动到初始化待机点");
    //移动到取生料点上方

    bool state = mainthread_robot_arm_->add_common_waypoint("取生料点上方",
                                                MoveType::LINE,
                                                LineSpeed,
                                                10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->add_common_waypoint("转移平台前方",
                                                    MoveType::LINE,
                                                    LineSpeed,
                                                    10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //移动回初始化待机点
    state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                    MoveType::JOINT,
                                                    JointSpeed,
                                                    10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    //发送转移平台允许移动信号
    log_message(LogType::LOG_INFO,"step-29: 发送转移平台允许移动信号");
    state = send_transfer_allowing_signal();

    //ui->doubleSpinBox_ct_5->setValue(stepTime.elapsed()/1000.0);

         current_step = 24;
}
//吸真空检测异常，有2中情况，没有物料，有铝屑没有吸起来
void MainWindow::run_step_23(){
    //吸真空检测异常，有2中情况，没有物料，有铝屑没有吸起来
    mainthread_robot_arm_->set_DO(6,0);

    //如果接近开关检测有物料
    if(check_by_BERO == 1){

        //返回待机点
        exception_when_taking_workpiece();

        QMessageBox::information(this, "提示\t", "机台有熟料，未吸起来\t");
        //停1s再弹出另外一个
        sleep_ms(1000);

        // 创建一个question弹出对话框，添加两个按钮：Yes和No
        QMessageBox *msgBox = new QMessageBox(QMessageBox::Question, tr("异常取料"), tr("是否继续CNC机台取料"),
                                              QMessageBox::Yes | QMessageBox::No);

        // 将原本显示“Yes”的按钮改为显示“是”
        msgBox->button(QMessageBox::Yes)->setText("是");
        // 将原本显示“No”的按钮改为显示“否”
        msgBox->button(QMessageBox::No)->setText("否");

        // 弹出对话框
        int ret = msgBox->exec();

        switch (ret) {
        case QMessageBox::Yes:
            qDebug() << "1";
            current_step = 11;
            break;
        case QMessageBox::No:
            qDebug() << "2";
            stop_work_flow();
            break;
        }
    }
    else{
        exception_when_no_workpiece_in_CNC();
        current_step = 24;
    }
}

void MainWindow::run_step_24(){
    //移动到相机拍照位
     log_message(LogType::LOG_INFO,"step-30: 移动到相机拍照位");
     //stepTime.start();

     QString type = calibration_.get_station_type(current_station_id);


     if(type == "左机台"){

         bool state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                     MoveType::JOINT,
                                                     JointSpeed,
                                                     10);


         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

       state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点二",
                                                   MoveType::LINE,
                                                   LineSpeed,
                                                   10);

       if(state == false || work_flag == false){
           stop_work_flow();
           return ;
       }


         //移动拍照待机点
       state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                           "相机拍照点",
                                                           MoveType::LINE,
                                                           LineSpeed,
                                                           10);

       if(state == false || work_flag == false){
           stop_work_flow();
           return ;
       }

       state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

       if(state == false || work_flag == false){
           stop_work_flow();
           return ;
       }

     }
     else if(type == "右机台")
     {
         //移动右边拍照待机点
           bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                       MoveType::JOINT,
                                                       JointSpeed,
                                                       10);

           if(state == false || work_flag == false){
               stop_work_flow();
               return ;
           }

         //移动右边拍照待机点
         state = mainthread_robot_arm_->add_station_waypoint(current_station_id,
                                                             "相机拍照点",
                                                             MoveType::LINE,
                                                             LineSpeed,
                                                             10);


         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

     }
     current_step = 25;
}

//视觉定位
void MainWindow::run_step_25(){
    //视觉拍照前，先关闭空压机
    NDC_Client_.write_AGV_data_68(1);

   //视觉定位
    log_message(LogType::LOG_INFO,"step-31: 视觉定位");

   bool state = process_image_and_correct_pose();

   //视觉拍照异常处理流程
   if(state == false){
       exception_when_processing_vision();

       stop_work_flow();
       log_message(LogType::LOG_ERROR,
                   current_station_id + "机台视觉拍照异常");
       QMessageBox::information(this, "提示\t", "视觉拍照异常\t");
       return;
   }

   //打开空压机
   log_message(LogType::LOG_INFO,"step-32: 打开空压机");
   NDC_Client_.write_AGV_data_68(0);

   current_step = 26;
}

void MainWindow::run_step_26(){
    //从相机拍照位，移动到二次定位平台
    log_message(LogType::LOG_INFO,"step-33: 从相机拍照位，移动到二次定位平台");
    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){
        //移动左边拍照待机点
        bool state = mainthread_robot_arm_->add_common_waypoint("左边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);


        if(state == false || work_flag == false){
            stop_work_flow();
            return;
        }

    }
    else if(type == "右机台"){

        bool state = mainthread_robot_arm_->add_common_waypoint("右边拍照待机点",
                                                            MoveType::LINE,
                                                            LineSpeed,
                                                            10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                            MoveType::JOINT,
                                                            JointSpeed,
                                                            10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

         //return state;
    }


    bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                       MoveType::LINE,
                                                       LineSpeed,
                                                       10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->add_common_waypoint("吸二次定位平台生料上方点",
                                                           MoveType::LINE,
                                                           LineSpeed,
                                                           10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }


    state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点",
                                                           MoveType::LINE,
                                                           15,
                                                           10);

    if(state == false){
        stop_work_flow();
        return ;
    }

    state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    current_step = 27;
}
//二次定位平台吸生料
void MainWindow::run_step_27(){
    //吸料
    log_message(LogType::LOG_INFO,"step-34: 吸料");
    bool state = suck_workpiece();
    //吸生料异常处理
    if(state == false){
        //关闭吸真空
        mainthread_robot_arm_->set_DO(6,0);
        //再吹一下，使物料落在二次定位平台
        chow_workpiece();
        //
        exception_when_taking_raw_at_localization();

        //stop_work_flow();
        QMessageBox::information(this, "提示\t", "二次定位平台取生料异常\t");
        //停1s再弹出另外一个
        sleep_ms(1000);

        // 创建一个question弹出对话框，添加两个按钮：Yes和No
        QMessageBox *msgBox = new QMessageBox(QMessageBox::Question, tr("异常取料"), tr("是否继续二次定位平台取料？"),
                                              QMessageBox::Yes | QMessageBox::No);

        // 将原本显示“Yes”的按钮改为显示“是”
        msgBox->button(QMessageBox::Yes)->setText("是");
        // 将原本显示“No”的按钮改为显示“否”
        msgBox->button(QMessageBox::No)->setText("否");

        // 弹出对话框
        int ret = msgBox->exec();

        switch (ret) {
        case QMessageBox::Yes:
            qDebug() << "1";
            current_step = 24;
            break;
        case QMessageBox::No:
            qDebug() << "2";
            stop_work_flow();
            break;
        }

        return;
    }

    current_step = 28;
    //ui->doubleSpinBox_ct_6->setValue(stepTime.elapsed()/1000.0);

}

void MainWindow::run_step_28(){

    //stepTime.start();

    //从二次定位平台，移动到CNC机台门前方
    log_message(LogType::LOG_INFO,"step-35: 从二次定位平台，移动到CNC机台门前方");
    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                        MoveType::LINE,
                                                                        LineSpeed,
                                                                        10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }


        state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

          state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                                MoveType::LINE,
                                                                LineSpeed,
                                                                10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                               MoveType::JOINT,
                                                               JointSpeed,
                                                               10);
         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("左边门前点二",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);
         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

    }
    else if(type == "右机台"){

        bool state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方过渡点",
                                                                        MoveType::LINE,
                                                                        LineSpeed,
                                                                        10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        state = mainthread_robot_arm_->add_common_waypoint("二次平台取料点上方",
                                                              MoveType::LINE,
                                                              LineSpeed,
                                                              10);
        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

         state = mainthread_robot_arm_->add_common_waypoint("二次平台上方待机点",
                                                               MoveType::LINE,
                                                               LineSpeed,
                                                               10);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                               MoveType::JOINT,
                                                               JointSpeed,
                                                               10);
         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

         state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }
    }

        current_step = 29;
    //ui->doubleSpinBox_ct_7->setValue(stepTime.elapsed()/1000.0);
}

void MainWindow::run_step_29(){
    //检测放料允许信号
    //stepTime.start();
    log_message(LogType::LOG_INFO,"step-36: 检测放料允许信号");
    waitfor_putting_raw_signal(-1);
    //qDebug()<< "等待放料允许信号时间：" << stepTime.elapsed()/1000.0 << "s";

        current_step = 30;
}

void MainWindow::run_step_30(){
    //检测光电信号，是否开门
    log_message(LogType::LOG_INFO,"step-37: 检测光电信号，是否开门");
    //stepTime.start();
    bool state = waitfor_opening_CNC_door(300000);
    if(state == false){
        exception_when_opening_door();
        stop_work_flow();
        QMessageBox::information(this, "提示\t", "CNC机台分钟未开门\t");
        return;
    }
    //qDebug()<< "等待开门光电检测时间：" << stepTime.elapsed()/1000.0 << "s";
    //ui->doubleSpinBox_ct_8->setValue(stepTime.elapsed()/1000.0);

        current_step = 31;

}

void MainWindow::run_step_31(){
    //移动到CNC机台内部放料位
    log_message(LogType::LOG_INFO,"step-38: 移动到CNC机台内部放料位");

    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->move_to_pose(station_correct_pose_above,
                                                MoveType::LINE,
                                                LineSpeed,LineACC,LineDEC,
                                                BlockMotion::LOCKED);

//          state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
//                                                              MoveType::LINE,
//                                                              LineSpeed,
//                                                              2);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    RobotPoseXYZ station_correct_pose_take_finished = station_correct_pose;

    //移动到CNC机台OK料位
    state = mainthread_robot_arm_->move_to_pose(station_correct_pose_take_finished,
                                                MoveType::LINE,
                                                50,15,15,
                                                BlockMotion::LOCKED);

//          state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_take_finished,
//                                                              MoveType::LINE,
//                                                              LineSpeed,
//                                                              10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

        current_step = 32;
}

void MainWindow::run_step_32(){
    //吹料
    log_message(LogType::LOG_INFO,"step-39: 吹料");
    mainthread_robot_arm_->set_DO(6,0);
    mainthread_robot_arm_->set_DO(3,1);

        current_step = 33;
}

void MainWindow::run_step_33(){

    //从CNC机台内部，移动回初始化待机点
    log_message(LogType::LOG_INFO,"step-40: 从CNC机台内部，移动回初始化待机点");
    //移动到CNC机台OK料位上方
    RobotPoseXYZ station_correct_pose_above = station_correct_pose;

    station_correct_pose_above.z += 20;//mm

    bool state = mainthread_robot_arm_->add_xyz_waypoint(station_correct_pose_above,
                                                         MoveType::LINE,
                                                         120,
                                                         10);

    if(state == false || work_flag == false){
        stop_work_flow();
        return ;
    }

    QString type = calibration_.get_station_type(current_station_id);

    if(type == "左机台"){

        state = mainthread_robot_arm_->add_common_waypoint("左边门前点",
                                                           MoveType::LINE,
                                                           LineSpeed,
                                                           10);


        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        //关闭吹料
        state = mainthread_robot_arm_->set_DO(3,0);
        //发送放料完成信号
        state = send_finishing_putting_signal();

        if(state == false){
            stop_work_flow();
            return;
        }

        //ui->doubleSpinBox_ct_total->setValue(totalTime.elapsed()/1000.0);

        state = mainthread_robot_arm_->move_to_common_pose("二次平台上方待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }

    }
    else if(type == "右机台"){

        state = mainthread_robot_arm_->add_common_waypoint("右边门前点",
                                                           MoveType::LINE,
                                                           LineSpeed,
                                                           10);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        state = mainthread_robot_arm_->move_by_path(BlockMotion::LOCKED);

        if(state == false || work_flag == false){
            stop_work_flow();
            return ;
        }

        //关闭吹料
        state = mainthread_robot_arm_->set_DO(3,0);
        //发送放料完成信号
        state = send_finishing_putting_signal();

        if(state == false){
            stop_work_flow();
            return;
        }

        ui->doubleSpinBox_ct_total->setValue(totalTime.elapsed()/1000.0);

         state = mainthread_robot_arm_->move_to_common_pose("初始化待机点",
                                                          MoveType::JOINT,
                                                          JointSpeed,JointACC,JointDEC,
                                                          BlockMotion::LOCKED);
         if(state == false || work_flag == false){
             stop_work_flow();
             return ;
         }
    }

    //ui->doubleSpinBox_ct_9->setValue(stepTime.elapsed()/1000.0);

    has_workpeice_in_localization = false;

    current_step = 0;
}


void MainWindow::on_pauseFlowButton_clicked()
{
    if(work_flag == false){
        QMessageBox::information(this, "提示\t", "流程尚未开启\t");
        return;
    }

    if(ui->pauseFlowButton->text() == "暂停流程")
    {
        qDebug() << "暂停流程";
        mainthread_robot_arm_->pause_movement();
        ui->pauseFlowButton->setText("恢复流程");
        ui->pauseFlowButton->setStyleSheet("background-color : rgb(0,255,0)");
    }
    else{
        //
        qDebug() << "恢复流程";
        mainthread_robot_arm_->run_movement();
        ui->pauseFlowButton->setText("暂停流程");
        ui->pauseFlowButton->setStyleSheet("background-color : rgb(255,255,0)");
    }
    return;
}


void MainWindow::on_checkBox_obstacle_stateChanged(int arg1)
{
    qDebug() << "on_checkBox_clicked " << arg1;

    if(arg1 == 2){

        if(timer_get_agv_state_->isActive()){
            qDebug() << "timer_get_agv_state_ already Active";
            return;
        }

        qDebug() << "timer_get_agv_state_ is Start";
        timer_get_agv_state_->start(500);
    }
    else if(arg1 == 0){

        if(timer_get_agv_state_->isActive()){

            qDebug() << "timer_get_agv_state_ is Stop";
            timer_get_agv_state_->stop();
            obstacle_state = false;
            return;
        }

        qDebug() << "timer_get_agv_state_ already Stop";
    }
}
